CN116257082B - Distributed active cooperative detection method for multiple unmanned aerial vehicles - Google Patents
Distributed active cooperative detection method for multiple unmanned aerial vehicles Download PDFInfo
- Publication number
- CN116257082B CN116257082B CN202310013673.XA CN202310013673A CN116257082B CN 116257082 B CN116257082 B CN 116257082B CN 202310013673 A CN202310013673 A CN 202310013673A CN 116257082 B CN116257082 B CN 116257082B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- cost
- boundary
- path
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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
- G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
-
- 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 multi-unmanned aerial vehicle distributed active cooperative detection method, wherein an unmanned aerial vehicle active detection strategy obtains the access sequence of all target points through solving the problem of traveling operators, a multi-unmanned aerial vehicle distributed hybrid A star front end track is planned to be a safe global path for each unmanned aerial vehicle, a multi-unmanned aerial vehicle distributed rear end track is optimized to optimize the global path, and finally map interaction among multiple unmanned aerial vehicles is completed through a multi-unmanned aerial vehicle distributed active cooperative detection system so as to construct a global map of the multiple unmanned aerial vehicles. By adopting the method, the unmanned aerial vehicle detection efficiency, the calculation speed and the safety performance are considered.
Description
Technical Field
The invention relates to the technical field of aircraft motion planning, in particular to a distributed active cooperative detection method for multiple unmanned aerial vehicles.
Background
The unmanned aerial vehicle can be divided into a fixed wing, an umbrella wing, a rotor wing and the like, wherein the four-rotor unmanned aerial vehicle has the advantages of low cost, light and handy structure, flexibility, good concealment and the like. In the current explosive period accompanied with the rapid development of the related technology of artificial intelligence, the variety of the technology is continuously increased, the performance is increasingly improved, and the application field is gradually expanded. As quad-rotor unmanned vehicles exhibit higher levels of autonomy, and can even be used as tools by people without special training, the number of unmanned vehicles in use increases rapidly. The low cost advantage of the drone allows it to perform frequent inspection or monitoring tasks, while the use of the drone in hazardous environments may reduce the risk of human exposure.
Aiming at the problem of active detection of unmanned aerial vehicles, a great deal of researches are carried out by students at home and abroad in various methods. Among them, the boundary-based method is one of classical methods, which was proposed by Yamauchi team in the united states in 1997, and then was evaluated comprehensively in Juli M team. To detect boundaries in three dimensions, the Shen team in 2012 has proposed a method based on stochastic differential equations. In 2017, the university of zurich, switzerland, cineslewski team focused on solving the problem of fast exploration of space of unmanned aerial vehicle, proposed a speed change minimization method, which in each decision would select the boundary with minimum speed change in the angle of view of unmanned aerial vehicle to maintain a consistently high flying speed, and the unmanned aerial vehicle detection efficiency was greatly improved. In 2020, deng team personnel introduced a boundary-based information gain micromeasured measure, allowing paths to be optimized using gradient information. Active detection based on sampling is another main method, the main idea of which is to randomly generate viewpoints to explore the space. This type of approach is closely related to the concept of NBV (Next-Best-View), which iteratively computes an overlay View to obtain a complete model of a scene. In 2016, the Bircher team in switzerland first used the NBV method in three-dimensional exploration, which extended the accessible space of the RRTs (Rapidly Exploring Random Trees) method and performed the boundaries with the highest information gain in a roll-optimized manner. The method is then continually improved, expanding on the consideration of different target requirements, such as positioning uncertainty, visual importance of different objects, and monitoring tasks. The Schmid team proposes to maintain and improve a single expansion tree continuously using a re-pruning scheme inspired by RRT (Rapidly Exploring Random Tree Star). To achieve faster flights, the Cieslewski team directly samples safe, dynamically viable sports waypoints and performs the largest one of the informations.
However, when the unmanned aerial vehicle performs an active detection task in complex scenes such as mine holes, dense jungles and the like, a simple detection algorithm is difficult to achieve a satisfactory effect. With the increase of space and the increase of obstacle complexity of task implementation scenes, active detection of a single unmanned aerial vehicle is difficult to be efficiently completed, and the active detection of the single unmanned aerial vehicle is gradually changed to multiple unmanned aerial vehicle directions. In order to reduce the exploration time and enable the unmanned aerial vehicle exploration system to have better robustness, the environment detection scheme of multi-unmanned aerial vehicle collaboration is focused on gradually, and the research focus of the multi-unmanned aerial vehicle collaboration exploration environment is to determine exploration paths of all robots so as to minimize the system exploration time and reduce repeated exploration of the same area, and meanwhile, coverage detection of an unknown environment is met. In recent years, yamauchi et al in the naval research laboratory in the United states have proposed a multi-robot exploration scheme in which each robot shares sensory information but independently decides exploration strategies, improving the fault tolerance of the system. Burgard et al, university of Frieburg, propose a probabilistic approach to reconciling multiple robots based on exploration costs and efficiency. Simmon et al, university of carnauba, propose a method of combining distributed computing with global decisions, each robot "bidding" according to its next expected cost and information gain to different viewpoints, a central server receives the bids and assigns tasks to minimize the total time of exploration, after which the team proposes a multi-unmanned autonomous environment exploration method based on collaborative boundaries based on the outdoor 3D environment. Recently, high-flying teams at Zhejiang university consider a centralized-distributed combination mode to perform multi-unmanned aerial vehicle collaborative detection, use one unmanned aerial vehicle to make decisions, allocate the decisions to other unmanned aerial vehicles for actively detecting target points, and perform distributed planning flight respectively. Most of the existing methods are greedy decisions, and the dynamics of the quadrotor aircraft are not considered, so that the flying efficiency is low and the maneuver conservation is achieved.
In summary, scholars at home and abroad have conducted a great deal of research on the problem of active detection of multiple unmanned aerial vehicles, and early research methods are mostly lack of long-range decision, and neglect the self-dynamics factors of unmanned aerial vehicles, so that the trajectories generated by unmanned aerial vehicles are not smooth, and the detection efficiency is greatly affected.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention provides a multi-unmanned aerial vehicle active detection method based on a boundary.
In order to achieve the above purpose, the technical scheme of the invention is as follows:
the distributed active cooperative detection method of the multiple unmanned aerial vehicles comprises the following steps:
a multi-unmanned aerial vehicle active detection strategy; the boundary cluster of the unmanned aerial vehicle is maintained in real time in an incremental computing mode, then the viewpoint with the most covering voxels of the unmanned aerial vehicle sensor is found in the boundary cluster, and the viewpoint is regarded as a target point in the current boundary cluster; calculating connection cost among all boundary cluster target points, designing a cost matrix conforming to the problem of the traveller on the basis of connection cost calculation, and obtaining the access sequence of all the target points by solving the problem of the traveller;
Planning the track of the front end of the multi-unmanned aerial vehicle distributed mixed A star; on the basis of solving boundary cluster information and target point access sequence, a mixed A star algorithm is used for planning a safe global path for each unmanned aerial vehicle;
optimizing the distributed back-end track of multiple unmanned aerial vehicles; and carrying out track optimization on the global path of each unmanned aerial vehicle based on a model prediction path integral algorithm of sampling, and adding obstacle avoidance constraint, tracking constraint, collision avoidance constraint and feasibility constraint into the track optimization to generate a track meeting specific requirements.
Further, the method also comprises the design of a multi-unmanned aerial vehicle distributed active cooperative detection system; on the basis of active detection of a single unmanned aerial vehicle, a multi-unmanned aerial vehicle active detection system with cooperative information interaction is designed to finish transfer of tracks, local maps and current position information among the multi-unmanned aerial vehicles; according to the local map of the unmanned aerial vehicle and the position information of the unmanned aerial vehicle, the current unmanned aerial vehicle calculates the reference detection update range of other unmanned aerial vehicles while maintaining the local global map, a global map with cooperative detection information of a plurality of unmanned aerial vehicles is constructed in a current-reference mode, and each unmanned aerial vehicle performs distributed cooperative detection on the basis of the global map with richer environmental information.
Further, the specific process of the multi-unmanned aerial vehicle active detection strategy comprises old boundary updating, new boundary generation, viewpoint calculation and cost matrix solving;
updating old boundaries; after each time the map is updated by the unmanned aerial vehicle sensor measurement data, the boundary box aligned with the axis of the updated area is recorded, all boundary clusters are traversed, the boundary clusters intersected with the updated area are returned, then the returned boundary clusters are checked, and voxel blocks which are not boundaries are removed;
generating a new boundary; searching and clustering the updated region based on a region growing algorithm used by a boundary method, traversing newly generated boundary clusters, omitting small clusters with small number of voxel blocks caused by noise, and clustering large clusters with large number of voxel blocks based on a principal component analysis method;
calculating a viewpoint; establishing a cylindrical coordinate system by taking the center of the boundary cluster as an origin, then obtaining a plurality of viewpoints in a unified sampling mode, and taking the viewpoint with the most covering voxels of the unmanned aerial vehicle sensor as a target point of the current boundary cluster;
solving a cost matrix; determining a connection cost equation among the boundary clusters, then establishing a travel business problem cost matrix of all the boundary clusters, and solving the cost matrix to obtain an access sequence of the target points;
Wherein T1 is the speed connection cost, and T2 is the yaw angle connection cost; t is t lb As a connection cost; m is M tsp A cost matrix;boundary clusters->Is a target point of (2); />Respectively->Coordinates of (c); />Is->Is arranged at the upper end of the frame; />Connection generated for the Path search algorithm>And->Is a safe path of the system;distance to the safe path; v max And->
Maximum limits for speed and yaw rate, respectively; n (N) cls Is the number of boundary clusters.
Furthermore, the cost matrix solution also introduces consistency cost and angle penalty weight coefficient to eliminate the incoherence of the flight process;
cost matrix M tsp First row and first column and second row of the current viewpoint position p 0 And yaw angleξ 0 The current viewpoint x of the composition 0 ＝(p 0 ,ξ 0 ) N cls The boundary clusters are related from x 0 Initially, the kth boundary cluster is calculated by,
wherein c (x k,1 ) Is a consistency cost; w (w) c The weight coefficient is punished for angles and is manually specified; is p k，1 ,v 0 Respectively the kth boundary cluster target point x k,1 And yaw rate.
In the multi-unmanned plane distributed mixed A star front end track planning, firstly, a Lattice diagram is built based on an unmanned plane model and discrete control input, then pruning operation is carried out on the Lattice diagram according to the principle that only one state point is reserved in a grid, and finally, path searching operation is carried out on the Lattice diagram after pruning, and an optimal path is searched out by using a process cost function and a heuristic function; the process cost function and the heuristic function and the total cost function are expressed as follows:
Wherein J is the accumulated path segment number, g (n) is a process cost function considering the previous J segment paths, and T is the specified elapsed time of each segment path; u (u) dj A control input for the j-th path; a ρ time importance parameter;an optimal cost function for connecting the current point to the target point; h (n) is a heuristic function, here taken to be equal to the optimal cost function C * (T h ) The method comprises the steps of carrying out a first treatment on the surface of the f (n) is the total cost function; (p) μc ,v μc ),(p μg ,v μg ) The positions and the speeds of the current point and the target point respectively; alpha μ ，β μ Optimizing parameters for the middle; t (T) h To C * Time T at which the value of (T) is minimum.
Furthermore, the grid map searching algorithm introduces an opportunistic expansion method, and the searching process is finished in advance after a safe global path is found.
Further, in the multi-unmanned aerial vehicle distributed back-end track optimization, introducing a value function of an optimal control problem, wherein the value of the function is influenced by the system state, the terminal cost and the operation cost; introducing random control input to obtain a local motion planning problem of the unmanned aerial vehicle, wherein the local motion planning problem is expressed in the form of the following generalized optimization problem:
in the method, in the process of the invention,for generalized random control of input sequences, V (x t T) is a value function; x is x t And u t The system state and the random control input of the unmanned aerial vehicle at the time t are respectively input; / >The four-rotor unmanned aerial vehicle dynamic model is provided; i 4 、I 6 The unit matrix is a fourth-order unit matrix and a sixth-order unit matrix respectively;
then, model predictive path integration randomly generates a control sequence U= { U with a fixed time domain length of T through a forward sampling method t ,…,u t+T -generating a predicted trajectory; calculating the state point and the total cost of each path on line, and judging whether the track is generated under the action of the input control sequence; for each input control sequence, the control sequence is composed of tracksThe cost-related index is used as weight calculation, and the optimal control sequence is a weighted sum, and the calculation formula is as follows:
u (i) ＝v+∈ (i)
wherein u is the initial control input of the current setting; k is the total number of control sequence samples; u (u) (i) Sampling an ith control sequence; omega i To give each control input a calculated weight; lambda is a custom parameter;the optimal control sequence is obtained by the model predictive path integral algorithm process of the previous round; e-shaped article (i) Sampling noise for the ith control; c (C) i The total path cost calculated for the ith sampling track is calculated by the track terminal cost +.>And forward propagation T MPPI The total running cost of the track of the step is formed; />For the path->Is the running cost of (a); v k A kth step control input in the optimal control sequence v; a is a control sampling covariance parameter; r is R k To control the sampling transformation matrix; />Sampling noise when the kth section is sampled; when one iteration is performed, the previous optimal control sequence is subjected to time shift and is used as the mean value of Gaussian distribution of sample control;
the operation cost is calculated by a cost function of dynamic feasibility constraint, tracking constraint, obstacle avoidance constraint and multi-machine collision avoidance constraint, and the operation cost is specifically as follows:
dynamic feasibility constraints:
tracking constraints:
obstacle avoidance constraint:
multiple inter-machine collision prevention constraint:
running cost:in (1) the->Is a state space; k (k) d Punishment for dynamic feasibility constraints; p is p t And v t Representing the position state and the speed state of the unmanned plane at the predicted time t; p is p des Is the desired positional state; />And->Weights for manually set position errors and velocity errors; d (x) t ) Is thatThe distance between the current position of the unmanned aerial vehicle and the nearest obstacle; />For safety distance->Is a dangerous distance; k (k) crash Punishment for obstacle avoidance constraints; f (f) p (d(x t ) Is an intermediate distance domain; d, d k,i (t) is the distance between the trajectories of the kth unmanned aerial vehicle and the ith unmanned aerial vehicle at the same time predicted by the kth unmanned aerial vehicle; t is t s And t e Is the track phi k Global start time and end time of the time span of (t); e=diag (1, 1/c), where c > 1 is the conversion constant.
In conclusion, the method has great significance for the research on the active detection technology of the four-rotor unmanned aerial vehicle in a multi-obstacle scene. By means of information interaction among multiple unmanned aerial vehicles and design of an active detection strategy and a motion planning method, collaborative autonomous detection tasks among the multiple unmanned aerial vehicles can be achieved, and therefore rapid active detection for complex scenes with large space and obstacles is achieved. The invention has very high theoretical and engineering values, and the advantages of the invention are summarized as follows:
(1) The invention provides an active detection strategy algorithm of multiple unmanned aerial vehicles. Aiming at the problem of limited computing power in the active detection process of the unmanned aerial vehicle, a boundary information structure capable of supporting the rapid detection of the unmanned aerial vehicle in a complex unknown environment is provided, boundary information processing modes such as clustering and computing connection cost are adopted for boundary voxels, and the rapid detection of boundary information is realized. On the basis, the boundary point access sequence solving is carried out by using the solving travel business problem, the complex travel business problem is simplified into an asymmetric travel business problem, the optimal target point sequence of the distance is obtained by solving, and the unmanned aerial vehicle is guided to fly and detect the boundary in an unknown environment;
(2) The invention provides a multi-unmanned aerial vehicle distributed hybrid A star front end track planning algorithm. Aiming at the unmanned aerial vehicle motion planning problem, after boundary target points are provided in the active detection strategy process, a mixed A star algorithm is used for front-end track planning, and a mixed A star algorithm path searching process is accelerated to generate an obstacle avoidance global track by reasonably designing heuristic functions and opportunistic expansion methods;
(3) The invention provides a multi-unmanned aerial vehicle distributed back-end track optimization algorithm. Aiming at the problem of poor feasibility of a global path, a model predictive path integral control strategy is provided for optimizing a front-end track. And analyzing the local track optimization problem, and converting the problem into an optimal control problem. And then, according to a core framework of model prediction control, carrying out local track solution by using model prediction path integration based on sampling. Finally, according to the real-time safety requirement of unmanned aerial vehicle flight, a series of safety constraint functions are designed, and a collision prevention function is additionally used for a multi-unmanned aerial vehicle system, so that cluster safety is achieved;
(4) The invention designs a distributed active cooperative detection system for multiple unmanned aerial vehicles. Aiming at the problem of low active detection efficiency of a single unmanned aerial vehicle, a multi-unmanned aerial vehicle active detection system with cooperative information interaction is designed on the basis of active detection of the single unmanned aerial vehicle. According to the distributed unmanned aerial vehicle local map, a current-reference mode is designed to construct the whole map of the multiple unmanned aerial vehicles, so that each unmanned aerial vehicle can quickly obtain multi-view map information, and collaborative quick active detection is realized;
(5) A complex closed simulation environment with multiple barriers is built in a Gazebo simulation environment, and the complex closed simulation environment comprises some barriers and walls. The simulation environment can be used for verifying the active detection method of the multiple unmanned aerial vehicles.
Drawings
FIG. 1 is a general structure diagram of distributed active cooperative detection of multiple unmanned aerial vehicles;
FIG. 2 is a schematic diagram of an active probing strategy of multiple unmanned aerial vehicles according to the present invention;
FIG. 3 is a schematic diagram of a method for planning a path of a front end of a distributed hybrid A star with multiple unmanned aerial vehicles;
FIG. 4 is a schematic diagram of a distributed back-end trajectory optimization for multiple unmanned aerial vehicles according to the present invention;
FIG. 5 is a schematic diagram of a distributed active cooperative detection system with multiple unmanned aerial vehicles according to the present invention;
FIG. 6 is a schematic diagram of incremental boundary information structure update according to the present invention;
FIG. 7 is a view point generation schematic diagram of the present invention;
FIG. 8 is a flow chart of the operation of the multiple unmanned system of the present invention;
FIG. 9 is a diagram of a closed multi-obstacle environment according to the present invention;
FIG. 10 is a diagram illustrating a multi-unmanned aerial vehicle position according to the present invention;
FIG. 11 is a diagram of an active cooperative detection process of multiple unmanned aerial vehicles according to the present invention;
FIG. 12 is a view of the collision avoidance effect of multiple unmanned aerial vehicles according to the present invention;
FIG. 13 is a map effect diagram generated by multiple unmanned aerial vehicles according to the present invention;
fig. 14 is a graph of a completion profile of active cooperative detection by multiple unmanned aerial vehicles according to the present invention.
Detailed Description
The technical scheme of the invention is further described below with reference to the accompanying drawings and examples.
The invention provides a boundary-based multi-unmanned aerial vehicle active detection method which mainly comprises a simulation environment part and an algorithm part. The simulation environment is built by Gazebo. Gazebo is powerful three-dimensional simulation software for robots, has good physical simulation performance, supports the real dynamics simulation of a plurality of high-performance physical engines, and has a vivid three-dimensional visualization effect. The simulation environment built by Gazebo is a closed enclosure space, and the interior of the simulation environment comprises a large number of columnar barriers. As shown in FIG. 1, the algorithm part mainly comprises four parts of a multi-unmanned aerial vehicle active detection strategy method, a multi-unmanned aerial vehicle distributed mixed A star front end path planning method, a multi-unmanned aerial vehicle distributed rear end track optimization algorithm and a multi-unmanned aerial vehicle distributed active cooperative detection system design.
The first part, the multi-unmanned plane active detection strategy, is shown in fig. 2.
And maintaining the boundary cluster of the unmanned aerial vehicle in real time in an incremental computing mode, finding the viewpoint with the most coverage voxels of the unmanned aerial vehicle sensor in the boundary cluster, and taking the viewpoint as a target point in the current boundary cluster. And calculating connection cost among all the boundary cluster target points, designing a cost matrix conforming to the problem of the traveller on the basis of calculation of the connection cost, and obtaining the access sequence of all the target points by solving the problem of the traveller.
Active boundary-based probing requires a significant amount of computational resources, and when planning paths in real-time, a fast and accurate re-planning of a path is critical to the security of the probing. In addition, efficient, instant updating of boundary information plays a key role for subsequent planning. Based on the above, a novel boundary information structure is used, which can be updated quickly and realize incremental calculation, and the structure is shown in fig. 6. Every time the map information is updated, it is detected whether the boundary cluster is affected, and if the boundary information is changed, the affected old boundary information is removed while detecting the boundary and acquiring the boundary information. When the boundary updating process is completed, a later detection planning process is started, the route points are generated, the connection cost is updated, then the route is planned, and the whole active detection process is ended when no boundary exists.
When a new boundary cluster F i Appears, a boundary information structure FI i Is calculated. The boundary information structure stores all voxel blocks C belonging to the boundary clusters i And C i Average position p of (2) iavg . In order to accelerate the detection process of boundary changes, an axis-aligned bounding box B i Will also be calculated; to complete the probe planning process, the candidate waypoints VP i Generated around the boundary clusters. In addition to all F i A linked list of bi-directional links associated with other boundary clusters is computed.
Each time the map is updated by sensor measurement data, the updated area B m Is aligned with the axis of bounding box B i It is also recorded that the old boundary clusters are removed and new ones are searched.
First traverse all boundary clusters and return to B m B with intersection of i Then a check is made for the returned boundary clusters to remove voxel blocks that are no longer boundaries. Both processes are inspired by collision detection algorithms, which has the advantage that unaffected clusters can be removed quickly and overconsumption is significantly reduced.
After the old boundary is removed, new boundaries are searched and clustered using a region growing algorithm similar to that used by conventional boundary-based methods. Among the newly searched boundary clusters, clusters with too small number of voxel blocks due to noise are ignored, and the rest of clusters may contain a huge number of clusters, which adversely affects the discrimination of unknown regions and the calculation of subsequent data. Thus, if the number of voxel blocks exceeds a threshold, principal Component Analysis (PCA) is used for the clusters and is divided into two uniform small clusters along the first principal axis. In order to break up all large boundary clusters into small clusters, the breaking up process is performed recursively.
Generating a boundary cluster means that the next detected target point needs to be generated, but the boundary cluster is a large area, and where the target point is in the boundary cluster can have a great influence on the detection efficiency. The central position of the boundary cluster is directly selected as the represented target point by the early-stage study, so that a lot of information of the boundary is ignored, and other planning decisions are adversely affected. Based on this situation, a method of collecting viewpoints around the boundary clusters is used, boundary information is fully utilized, and the target point position is determined in a more intelligent manner.
When a new boundary cluster is generated, a series of view VP's are generated i ＝{x i,1 ,x i,2 ,...,x i,n Cover it, where x i,j (j=1,., n) represents the j-th viewpoint generated in the i-th boundary cluster, as shown in fig. 7. At the same time, the VP considers the influence of the detection range of the sensor on the detection process i Not only the coordinate information of the basic point is contained, but also the yaw angle xi of the unmanned aerial vehicle at the point is contained. Based on the sampling radius r and the angle θ, the coordinates of the newly sampled point in the coordinate system are (r·cos (Δθ), r·sin (Δθ)), then it is determined whether the sampled point is within the marked bounding box of the map, unoccupied, not located near the unknown region, and when the above conditions are both yes, the yaw angle is calculated and the sampled point is recorded. For each possible recorded sampling point, the yaw angle requirement is that the coverage of the sensor is maximum, and the corresponding optimal pitch angle is calculated by using a yaw angle optimization method . The quality of the coverage is determined by the number of unoccupied boundary voxels that can be detected, and each view is counted and arranged in descending order, and views above a certain standard are finally preserved. It should be noted that at most N is reserved max And the view points.
In order to implement the planning of the global probing path, the connection cost between each pair of boundary clusters needs to be calculated. UsingRepresenting two viewpoints +.>To->The lower limit of the movement time between the two, namely the connection cost, is calculated by the formula (1):
wherein:
wherein T1 is the speed connection cost, and T2 is the yaw angle connection cost;boundary clusters->The viewpoint of the inner maximum coverage area is the target point; />Respectively->Coordinates of (c); />Connection generated for the Path search algorithm>And->Is a safe path of the system; />Distance to the safe path; v max And->Maximum limits for speed and yaw rate, respectively.
The cost is calculated using an incremental approach, and when an old boundary cluster is removed, the cost associated with it is also removed. The connection costs of the remaining boundary clusters are then recalculated.
The detection planning process aims at efficiently finding a path which can cover all current boundary clusters, the most classical task allocation problem sets the evaluation standard to be the minimum sum of the paths reaching each point, and in unmanned aerial vehicle problems, not only path problems but also speed and track smoothness of the unmanned aerial vehicle are required to be considered, so that the speed is considered when the connection cost is calculated. The problem is a modified traveller problem which is reduced to a standard asymmetric traveller problem (ATSP) in order to make the calculation process more efficient. By properly designing the input condition cost matrix of the problem, and using existing classical algorithms, the problem can be solved quickly.
Assuming a total of N cls Boundary clusters，M tsp Is N cls A square matrix of +1 dimensions. The matrix is composed of a main part of each pair of boundary clusters (e.g. boundary clusters) The calculated connection cost is formed by the following expression modes:
the cost calculation in the above formula adopts an incremental mode, and the cost is calculated every time a new boundary is detected. And the above process proceeds whenever there is a boundary change to be detected. Thus, the N is cls ×N cls The part of the dimension that consists of the connection cost only needs to be populated with the previously calculated values and does not create additional computational burden.
M tsp And the first row and first column of (1) and the currently obtained viewpoint x 0 ＝(p 0 ,ξ 0 ) N cls The boundary clusters are related, wherein p 0 Being the position of the current point, ζ 0 The unmanned yaw angle for the current point mentioned earlier. From x 0 Initially, the kth boundary cluster is calculated by equation (5):
wherein x is k,1 Is the kth boundary cluster target point.
Note that, in the above equation, a consistency cost c (x k,1 ) The partial cost is calculated using equation (6):
in the formula, v 0 Is the current unmanned plane speed, p k，1 Is the kth boundary cluster target point position. In some cases, multiple flights of a flightThere is a considerable lower bound of time, so a back and forth motion occurs in successive planning phases and slows down the process. The algorithm introduces an angle penalty weight coefficient w c The corresponding angle of the flight direction is calculated, and a great penalty is applied to the condition of great change, so that the inconsistency of the process is eliminated.
Using this method, the drone's flight back from a target point to the current point of view does not create additional costs during any flight, thus indicating that each closed loop trip in the problem contains an open loop trip of the same cost. For open loop solution we can use the traditional TSP problem to obtain the optimal open loop trip by solving the optimal closed loop trip and solving the equivalent open loop trip based on the same cost premise.
And in the second part, the flow of the multi-unmanned aerial vehicle distributed hybrid A star front end path planning method is shown in figure 3.
And the front-end track planning maintains a map in the planning process according to the idea of map searching, and adds and deletes grid points in the map and updates cost by applying the set cost judgment standard. In order to better adapt to the power demand of an unmanned aerial vehicle, planning is considered by combining with a Lattice diagram. The Lattice planning needs to construct a search graph, firstly, a unmanned plane model is given, and the control space is discretized. In a feasible control amount interval, the control amount is divided into n equal parts, each control amount is input into the system, and the system is driven forward at a given time, so that a very dense Lattice diagram is obtained. But the Lattice diagram has problems: many paths come together, probably hit the same obstacle; nonsensical sampling would make the map very dense, but in practice not so much multipath is required. Therefore, considering a pruning of the graph, an intuitively effective method for pruning the graph is to prune the graph by using the grid map. The search algorithm of the grid map is combined with the Lattice map to be the hybrid A star algorithm.
Firstly, introducing a specific pruning process, assuming an empty grid, and initially, a state of the robot falls in the grid under the drive of a control quantity, and storing the state; then a new state falls in the grid under the drive of other control quantity, and the state with low cost is reserved according to the cost of the two states. In the searching process, different control input quantities are selected, the forward driving system integrates forward, and the state of only one robot is recorded in the grid of each grid map, so that the usefulness of a Lattice diagram is maintained, and redundant paths before improvement are avoided. Then, when expanding neighbor nodes, the hybrid A star algorithm is based on a Lattice diagram, and the adjacent state generated by the traversal system after control input driving is traversed instead of simple space structure traversal.
In addition, the cost function of the hybrid A star algorithm needs to comprehensively consider the path distance and the motion process corresponding to the characteristic of the hybrid A star algorithm that the dynamics are considered.
Process cost function
The algorithm defines a function C (T) that takes into account the time cost and the control cost:
where T is the elapsed time specified for each path.
Since the same control input u (T) is applied for a given period T at a time, the connection between every two states is at the cost of:
C(T)＝(||u d || 2 +ρ)T (8)
wherein u is d For an applied control input that has been discretized, ρ is a time importance parameter that measures this trajectory, the larger ρ the greater the effect that time takes into account when generating the trajectory, but also affects the smoothness of the path. Assuming that the currently explored part of the optimal path passes through J state points, the control input of the jth path is u dj The process cost of the neighbor node which is currently expanded can be calculated by the formula (7) and the formula (8)
(II) heuristic function
When the heuristic function h (n) value is selected in the algorithm, an ideal track with the minimum C (T) value from the current state to the target state is calculated. Using the pointrisia minimum principle:
wherein p is μc ,v μc For the position and speed of the current point, p μg ,v μg Is the position and velocity of the target point.Is the optimal cost defined by equation (11). To find the optimal time T, at C * (T) adding an intermediate optimization parameter alpha μ ,β μ And find +.>This may be C * The root with the smallest value of (T) is denoted as T h 。
Using C in the present algorithm * (T) as a heuristic h (n). From this, the total cost f (n) for each grid point on the map is derived by equation (12):
f(n)＝g(n)+h(n)＝g(n)+C * (T h ) (12)
The value of the heuristic function calculated by the method meets feasibility and is close to the optimal solution, so that the number of certain expansion states is reduced, and the speed of path searching is increased.
Meanwhile, in order to complete the whole searching process more quickly, the algorithm uses opportunistic expansion. In the process of expanding the search tree, a theoretical optimal path from the current point to the end point is solved with a certain probability, and if the path just meets dynamics and does not collide with an obstacle, the path searching process can be finished in advance. Based on the above consideration, when each expanded node starts to search a path within a certain distance range close to a target point, the method described in the previous section is used for calculating the optimal track from the current node to the target node, and then whether the track meets the safety and the dynamic feasibility is judged, and if the track meets the safety and the dynamic feasibility, the searching process is terminated in advance. Due to the high success rate of such attempts when approaching the target point, the method greatly improves the efficiency of the hybrid a-star search process.
And the third part, the multi-unmanned aerial vehicle distributed back-end track optimization method, and the flow of the multi-unmanned aerial vehicle distributed back-end track optimization method is shown in fig. 4.
Considering differential flatness of the unmanned aerial vehicle, a flat output of the quadrotor unmanned aerial vehicle is selected as [ p ] x ,p y ,p z ,ψ] T Wherein p is x ,p y ,p z The following four-rotor unmanned aerial vehicle dynamic model can be obtained when the positions of x, y and z directions are respectively x, y and z are the yaw angles of the unmanned aerial vehicle:
wherein I is 4 、I 6 Respectively a fourth-order identity matrix and a sixth-order identity matrix, x t And u t And the system state and the control input of the unmanned aerial vehicle at the time t are respectively. System state x t Position p comprising xyz three directions of unmanned aerial vehicle in three-dimensional space x ,p y ,p z Velocity v x ,v y ,v z Acceleration a x ,a y ,a z And the yaw angle ψ, thereby yielding x t ＝[p x ,p y ,p z ,v x ,v y ,v z ,a x ,a y ,a z ,ψ] T Control of the power supplyInput u t ＝[u x ,u y ,u z ,u ψ ] T Similarly comprises displacement accelerations v in three directions x ,v y ,v z Yaw rate u ψ 。
When considering a path integration algorithm, a value function V (x) introducing an optimal control problem t T), the value of the function is affected by the system state, the terminal cost, and the running cost. Introducing random control input to obtain a local motion planning problem of the unmanned aerial vehicle, wherein the local motion planning problem is expressed in the form of the following generalized optimization problem:
wherein u is t Is the control input u of the system t Input v obtained by sampling t The sum of the results of the addition,is the last derived random control input sequence { u } t ,...,u t+T }. With the above process, the problem of local motion planning of the multi-rotor unmanned aerial vehicle is converted into the problem of optimal control in a fixed time domain. The process provides convenience for realizing the local motion planning, namely, the optimal control sequence is solved, so that the problem of the local motion planning is solved quickly and effectively.
The model prediction path integration is a random model prediction control algorithm based on sampling, and a control sequence U= { U with a fixed time domain length of T is randomly generated by a forward sampling method t ,…,u t+T The method comprises the steps of generating a predicted track, calculating the state point sum total cost of each path on line, wherein the cost is a sum of considered set cost and comprises a cost formed by a plurality of soft constraints, and judging whether the track is generated under the action of an input sequence. For each input sequence, calculated by the track cost related index as a weight, the optimal control sequence is a weighted sum calculated by equation (15):
u (i) ＝v+∈ (i) (17)
wherein u is the initial control input of the current setting, K is the total number of control sequence samples, u (i) Is the ith control sequence sample, w i Is a calculated weight given to each control input,is the optimal control sequence obtained by the previous round of model predictive path integral algorithm process, and is used for controlling sampling noise epsilon through the control sequence v (i) Obtaining u (i) 。C i The total path cost calculated by the ith sampling track is the track terminal cost +.>And forward propagation T MPPI Track total running cost of steps is formed, +.>Is a path->V of the operation cost of (v) k For the kth control input in the optimal control sequence v, a is the control sample covariance parameter, R k Is a control sampling transformation matrix,/, for>Sampling noise at the time of the kth segment sampling. At the next iteration, the previous one will be the mostThe optimal control sequence is time-shifted and used as the mean value of the gaussian distribution of the sample control. />
The motion process of the unmanned aerial vehicle is limited by constraints from the aspects of flight safety and the like, and one great advantage of the model prediction path integration algorithm is that the perception constraints and various optimization targets can be integrated to obtain a complex nonlinear objective function, so that the complex local planning problem is converted into a random optimal control problem. Considering the system bearing capacity of the unmanned aerial vehicle, the target point requirement of the motion process and the limitation of a series of safe flights, the algorithm provides a method for adding dynamic feasibility constraint, tracking constraint, obstacle avoidance constraint and collision avoidance constraint which needs to be considered among multiple unmanned aerial vehicles:
(1) Dynamic feasibility constraints
One planned trajectory needs to take into account the dynamic feasibility of the unmanned aerial vehicle, and generally needs to make the speed and acceleration of the unmanned aerial vehicle within the allowable range of the dynamics of the unmanned aerial vehicle model, namely the speed v at each moment t With acceleration a t To meet maximum speed v max And maximum acceleration a max And (5) limiting. Therefore, the unmanned aerial vehicle needs to limit the speed and the acceleration in the flight process, and the constraint is added:
||v t ||＜v max ,||a t ||＜a max (19)
When using the model predictive path integration algorithm, the constraint is converted into a discontinuous cost function q d ：
Wherein x is t Is the unmanned state at each moment in time,for a state space consisting of limits of speed and acceleration, the parameter k d For penalizing status points of infeasible predicted trajectories.
(2) Tracking constraints
The unmanned plane designs a cost function q of the difference between a predicted state and an expected state by taking a target point of a local track as a consideration factor in the algorithm according to the basic requirement that the actual arrival of the predicted target point is in a motion process t ：
Wherein p is t And v t Representing the position state and the speed state of the unmanned plane at the predicted time t, p des Is the desired position state, parameterAnd->Is the weight of the position error and the speed error manually set in the cost function.
(3) Obstacle avoidance restraint
The flight process of the unmanned aerial vehicle needs to consider surrounding obstacle information in real time, and in order to ensure safety, a cost function q of obstacle avoidance is designed according to the distance between the unmanned aerial vehicle and the nearest obstacle c ：
Wherein d (x t ) And the distance between the current position of the unmanned aerial vehicle and the nearest obstacle is represented. When the distance is greater than the set safety rangeWhen the cost function ignores the collision prevention effect. When the state at the predicted time is less than the dangerous distance +. >The cost function is obtained by adding a maximum cost k crash As a penalty for this,and forcibly stops the forward propagation in that direction. Considering the obstacle position to be predicted and planning a trajectory away from the obstacle in advance, the cost function is set to a state d (x t ) Located in the field->The inner part is a monotonically decreasing function along with the distance between the inner part and the nearest obstacle, and a function f in the form of a power function with the order of k more than 0 is selected p (d(x t ) As a calculation basis for the intermediate distance field: />
(4) Collision prevention constraint among multiple machines
When a plurality of unmanned aerial vehicles perform tasks together, due to the distributed structure of the unmanned aerial vehicles, the problem that collision among unmanned aerial vehicles is caused by other unmanned aerial vehicles which come before the unmanned aerial vehicles fly can be generated, so that collision avoidance constraint of the unmanned aerial vehicles needs to be considered, the algorithm mutually receives and transmits track information planned by the unmanned aerial vehicles, and simultaneously synchronizes a time axis, and a collision avoidance cost function is designed by considering track information calculated by the previous round of the other unmanned aerial vehicles when a new track is calculated. Assuming three-dimensional spaceIs->Is the position state of the kth frame at time t in the total K-frame unmanned aerial vehicle group, +.>The kth unmanned aerial vehicle calculates free space in consideration of the existence of other unmanned aerial vehicles. Thereby(s) >Wherein the method comprises the steps ofIs the total state space and each drone generates a flight trajectory by comparing the distances of the flight trajectories received within the same flight time as the surrounding drones.
Similar to obstacle avoidance constraint and dynamic feasibility constraint, the algorithm designs a cost function q for multi-machine collision avoidance for the unmanned aerial vehicle w,k As a one-way soft constraint:
wherein d k,i (t) is the distance between the trajectories of the kth unmanned aerial vehicle and the ith unmanned aerial vehicle at the same time, t s And t e Is the track phi k Global start time and end time of the time span of (t); e=diag (1, 1/c), where c > 1 is a conversion constant for converting euclidean distance to shorter ellipsoidal distance of principal axis at z-axis to reduce the risk of downward flight dive.
Thus, various soft constraints are added to the model predictive path integration algorithm, the total path cost C i Cost of operation in (a)
Finally, the monte carlo approximation of the path integration can make full use of CUDA (Compute Unified Device Architecture) computational structure. The whole main calculation process only needs to refer to the shared memory and the register memory, and uses a limited number of global memory read-write operations. And the running speed of a model predictive path integral algorithm is accelerated in a GPU (Graphic Processing Unit) parallel computing mode, and the unmanned aerial vehicle plans a safe path at a high frequency.
The fourth part, the design method of the distributed active cooperative detection system of the multiple unmanned aerial vehicles, is shown in fig. 5.
The clustered unmanned aerial vehicle is mainly divided into a distributed architecture and a centralized architecture, a multi-unmanned aerial vehicle architecture based on the distributed architecture is adopted in the design, a brief flow chart of the whole multi-unmanned aerial vehicle system is shown in fig. 8, each unmanned aerial vehicle receives transfer track information, local map information and current position information, then a boundary detection strategy algorithm is used for determining a boundary detection target point, then a front-end planning algorithm is used for generating a global path, and a rear-end track optimization algorithm is used for optimizing front-end tracks and other operations. Because of the distributed characteristics, each unmanned aerial vehicle in the unmanned aerial vehicle system does not interfere with each other except the information which is necessarily transmitted. Therefore, if a problem occurs in one unmanned aerial vehicle, the unmanned aerial vehicle cannot influence other unmanned aerial vehicles, and the task can be normally completed. The plurality of unmanned aerial vehicles are an unmanned aerial vehicle system, each unmanned aerial vehicle is an independent individual, and the unmanned aerial vehicle receives and transmits information through the unmanned aerial vehicle and calculates a path (local path) to be taken next by the unmanned aerial vehicle. Meanwhile, some cooperative operations, such as necessary multi-machine collision prevention constraint, are completed, unmanned planes are required to mutually transmit track information at the same moment, time synchronization is used for the track information, corresponding calculation is carried out on the cost of the track information, and the track information is used for track planning at the next moment of a certain unmanned plane. Each unmanned aerial vehicle receives the transfer track information, the local map information and the current position information, then uses an active detection strategy and a track optimization algorithm to perform operations such as environment detection, target point selection, rear end track optimization and the like, and because of the distributed characteristics, each unmanned aerial vehicle in the unmanned aerial vehicle system does not interfere with each other except the information which is necessarily transmitted, if a problem occurs in one unmanned aerial vehicle, the unmanned aerial vehicle cannot influence other unmanned aerial vehicles, and the task can be normally completed.
For collaborative detection of multiple unmanned aerial vehicles, map transmission is also needed between unmanned aerial vehicles, and each unmanned aerial vehicle mutually receives and transmits each built local map and local boundary so as to update the whole global boundary voxel map, so that great improvement of efficiency can be brought. On the basis of maintaining global map and boundary point information of each unmanned aerial vehicle, the unmanned aerial vehicle simultaneously supposes a reference process of other unmanned aerial vehicles according to position information sent by other unmanned aerial vehicles, calculates an updating range of the unmanned aerial vehicle, and updates boundary points of other unmanned aerial vehicles according to the updated global boundary voxel map and the updating range, so that one unmanned aerial vehicle can completely and independently calculate relevant information of the whole environment system by using a current-reference mode.
The active detection of multiple unmanned aerial vehicles is carried out in the environment shown in fig. 9, and the initial position of the unmanned aerial vehicle is shown in fig. 10. The verification adopts three unmanned aerial vehicles UAV1, UAV2 and UAV3 to perform distributed active detection, the size of a designed closed environment is 51.5m by 60m, and the algorithm is applied to simulation. The active detection process of three unmanned aerial vehicles in the Gazebo environment is shown in fig. 11, each unmanned aerial vehicle searches for construction environment information towards an unknown area, and fig. 11 (1) (2) (3) (4) is a process diagram of active detection of the unmanned aerial vehicle in a simulation environment when the unmanned aerial vehicle is 100s, 200s, 300s and 400s respectively. The collision prevention effect between multiple unmanned aerial vehicles is shown in fig. 12. The multi-unmanned aerial vehicle system constructs a multi-angle global map through mutual transmission of local maps, a display result of the multi-angle global map in Rviz is shown in fig. 13, wherein fig. 13 (1) is a map constructed when 3 unmanned aerial vehicles just start active collaborative detection, fig. 13 (1) (2) (3) is a local global map display constructed at the 100s, 300s and 400s moments in the unmanned aerial vehicle active detection process, and fig. 13 (4) is a global map of the whole environment constructed when 3 unmanned aerial vehicles finally finish active detection. The figures show that the three unmanned aerial vehicles continuously exchange the map in the flight process, and finally complete the complete detection of the whole environment. In this 1000 m environment, three unmanned aerial vehicles take about 600s to complete the detection of the whole space, and the detection process completion curve is shown in fig. 14.
Based on the four parts, the design of the active detection algorithm of the multiple unmanned aerial vehicles is completed. When a specific task is executed, the simulation environment can be modified according to specific requirements, and the number of unmanned aerial vehicles is correspondingly modified to obtain better detection efficiency. And in the aspect of safety constraint, the coefficients in a series of cost equations can be flexibly adjusted according to the safety strictness, so that the calculation speed and the safety performance are both realized.
The foregoing is a specific embodiment of the present invention, but the scope of the present invention should not be limited thereto. Any changes or substitutions that would be obvious to one skilled in the art are deemed to be within the scope of the present invention, and the scope is defined by the appended claims.
Claims (4)
1. The distributed active cooperative detection method for the multiple unmanned aerial vehicles is characterized by comprising the following steps of:
a multi-unmanned aerial vehicle active detection strategy; the boundary cluster of the unmanned aerial vehicle is maintained in real time in an incremental computing mode, then the viewpoint with the most covering voxels of the unmanned aerial vehicle sensor is found in the boundary cluster, and the viewpoint is regarded as a target point in the current boundary cluster; calculating connection cost among all boundary cluster target points, designing a cost matrix conforming to the problem of the traveller on the basis of connection cost calculation, and obtaining the access sequence of all the target points by solving the problem of the traveller; the specific process of the multi-unmanned aerial vehicle active detection strategy comprises old boundary updating, new boundary generating, viewpoint calculating and cost matrix solving;
Updating old boundaries; after each time the map is updated by the unmanned aerial vehicle sensor measurement data, the boundary box aligned with the axis of the updated area is recorded, all boundary clusters are traversed, the boundary clusters intersected with the updated area are returned, then the returned boundary clusters are checked, and voxel blocks which are not boundaries are removed;
generating a new boundary; searching and clustering the updated region based on a region growing algorithm used by a boundary method, traversing newly generated boundary clusters, omitting small clusters with small number of voxel blocks caused by noise, and clustering large clusters with large number of voxel blocks based on a principal component analysis method;
calculating a viewpoint; establishing a cylindrical coordinate system by taking the center of the boundary cluster as an origin, then obtaining a plurality of viewpoints in a unified sampling mode, and taking the viewpoint with the most covering voxels of the unmanned aerial vehicle sensor as a target point of the current boundary cluster;
solving a cost matrix; determining a connection cost equation among the boundary clusters, then establishing a travel business problem cost matrix of all the boundary clusters, and solving the cost matrix to obtain an access sequence of the target points;
wherein T1 is the speed connection cost, and T2 is the yaw angle connection cost; t is t lb As a connection cost; m is M tsp A cost matrix;boundary clusters->Is a target point of (2); />Respectively->Coordinates of (c); />Is->Is arranged at the upper end of the frame; />Connection generated for the Path search algorithm>And->Is a safe path of the system;distance to the safe path; v max And->Maximum limits for speed and yaw rate, respectively; n (N) cls Is the number of boundary clusters;
planning the track of the front end of the multi-unmanned aerial vehicle distributed mixed A star; on the basis of solving boundary cluster information and target point access sequence, a mixed A star algorithm is used for planning a safe global path for each unmanned aerial vehicle; in the multi-unmanned-plane distributed mixed A star front end track planning, firstly, a Lattice diagram is built based on an unmanned plane model and discrete control input, then pruning operation is carried out on the Lattice diagram according to the principle that only one state point is reserved in one grid, and finally, path searching operation is carried out on the Lattice diagram after pruning, and an optimal path is searched out by using a process cost function and a heuristic function; the process cost function and the heuristic function and the total cost function are expressed as follows:
wherein J is the accumulated path segment number, g (n) is a process cost function considering the previous J segment paths, and T is the specified elapsed time of each segment path; u (u) dj A control input for the j-th path; a ρ time importance parameter;an optimal cost function for connecting the current point to the target point; h (n) is a heuristic function, here taken to be equal to the optimal cost function C * (T h ) The method comprises the steps of carrying out a first treatment on the surface of the f (n) is the total cost function; (p) μc ,v μc ),(p μg ,v μg ) The positions and the speeds of the current point and the target point respectively; alpha μ ，β μ Optimizing parameters for the middle; t (T) h To C * Time T at which (T) is minimum;
optimizing the distributed back-end track of multiple unmanned aerial vehicles; track optimization is carried out on the global path of each unmanned aerial vehicle based on a sampling model prediction path integral algorithm, and obstacle avoidance constraint, tracking constraint, collision avoidance constraint and feasibility constraint are added into the track optimization to generate a track meeting specific requirements; introducing a value function of an optimal control problem in the distributed back-end track optimization of the multiple unmanned aerial vehicles, wherein the value of the function is influenced by the system state, the terminal cost and the running cost; introducing random control input to obtain a local motion planning problem of the unmanned aerial vehicle, wherein the local motion planning problem is expressed in the form of the following generalized optimization problem:
in the method, in the process of the invention,for generalized random control of input sequences, V (x t T) is a value function; x is x t And u t The system state and the random control input of the unmanned aerial vehicle at the time t are respectively input; / >The four-rotor unmanned aerial vehicle dynamic model is provided; i 4 、I 6 The unit matrix is a fourth-order unit matrix and a sixth-order unit matrix respectively;
then, model predictive path integration randomly generates a control sequence U= { U with a fixed time domain length of T through a forward sampling method t ,…,u t+T -generating a predicted trajectory; calculating the state point and the total cost of each path on line, and judging whether the track is generated under the action of the input control sequence; for each input control sequence, the index related to the track cost is used as a weight to calculate, and the optimal control sequence is a weighted sum, and the calculation formula is as follows:
u (i) ＝v+∈ (i)
wherein u is the initial control input of the current setting; k is the total number of control sequence samples; u (u) (i) Sampling an ith control sequence; omega i To give each control input a calculated weight; lambda is a custom parameter;the optimal control sequence is obtained by the model predictive path integral algorithm process of the previous round; e-shaped article (i) Sampling noise for the ith control; c (C) i The total path cost calculated for the ith sampling track is calculated by the track terminal cost +.>And forward propagation T MPPI The total running cost of the track of the step is formed; />For the path->Is the running cost of (a); v k A kth step control input in the optimal control sequence v; a is a control sampling covariance parameter; r is R k To control the sampling transformation matrix; />Sampling noise when the kth section is sampled; when one iteration is performed, the previous optimal control sequence is subjected to time shift and is used as the mean value of Gaussian distribution of sample control;
the operation cost is calculated by a cost function of dynamic feasibility constraint, tracking constraint, obstacle avoidance constraint and multi-machine collision avoidance constraint, and the operation cost is specifically as follows:
dynamic feasibility constraints:
tracking constraints:
obstacle avoidance constraint:
multiple inter-machine collision prevention constraint:
running cost:
in the method, in the process of the invention,is a state space; k (k) d Punishment for dynamic feasibility constraints; p is p t And v t Representing the position state and the speed state of the unmanned plane at the predicted time t; p is p des Is the desired positional state; />And->Weights for manually set position errors and velocity errors; d (x) t ) The distance between the current position of the unmanned aerial vehicle and the nearest obstacle is the distance; />For safety distance->Is a dangerous distance; k (k) crash Punishment for obstacle avoidance constraints; f (f) p (d(x t ) Is an intermediate distance domain; d, d k,i (t) is the distance between the trajectories of the kth unmanned aerial vehicle and the ith unmanned aerial vehicle at the same time predicted by the kth unmanned aerial vehicle; t is t s And t e Is the track phi k Global start time and end time of the time span of (t); e=diag (1, 1/c), where c > 1 is the conversion constant.
2. The detection method of claim 1, further comprising a multi-unmanned distributed active cooperative detection system design; on the basis of active detection of a single unmanned aerial vehicle, a multi-unmanned aerial vehicle active detection system with cooperative information interaction is designed to finish transfer of tracks, local maps and current position information among the multi-unmanned aerial vehicles; according to the local map of the unmanned aerial vehicle and the position information of the unmanned aerial vehicle, the current unmanned aerial vehicle calculates the reference detection update range of other unmanned aerial vehicles while maintaining the local global map, a global map with cooperative detection information of a plurality of unmanned aerial vehicles is constructed in a current-reference mode, and each unmanned aerial vehicle performs distributed cooperative detection on the basis of the global map with richer environmental information.
3. The method of claim 1, wherein the cost matrix solution also introduces a consistency cost and an angle penalty weight coefficient to eliminate the inconsistency of the flight process;
cost matrix M tsp First row and first column and second row of the current viewpoint position p 0 And yaw angle xi 0 The current viewpoint x of the composition 0 ＝(p 0 ,ξ 0 ) N cls The boundary clusters are related from x 0 Initially, the kth boundary cluster is calculated by,
wherein c (x k,1 ) Is a consistency cost; w (w) c The weight coefficient is punished for angles and is manually specified; is p k，1 ,v 0 Respectively the kth boundary cluster target point x k,1 Position and yaw of (2)Angular velocity.
4. The probing method of claim 1, wherein the search algorithm of the grid map introduces an opportunistic expansion method, and the search process is terminated in advance after a safe global path is found.
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN2022113692005 | 2022-11-03 | ||
| CN202211369200 | 2022-11-03 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116257082A CN116257082A (en) | 2023-06-13 |
| CN116257082B true CN116257082B (en) | 2023-09-22 |
Family
ID=86683604
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310013673.XA Active CN116257082B (en) | 2022-11-03 | 2023-01-05 | Distributed active cooperative detection method for multiple unmanned aerial vehicles |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116257082B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116774735B (en) * | 2023-08-24 | 2023-11-10 | 北京理工大学 | Unmanned aerial vehicle cluster track planning method and system based on edge calculation |
| CN117170406B (en) * | 2023-09-21 | 2024-04-12 | 四川大学 | Unmanned aerial vehicle rapid autonomous searching method based on hierarchical planning |
| CN117289721A (en) * | 2023-11-24 | 2023-12-26 | 天津云圣智能科技有限责任公司 | Obstacle avoidance path planning and mapping method and device, electronic equipment and storage medium |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| AU2013101142A4 (en) * | 2013-08-28 | 2013-10-03 | Parker, Scott Charles MR | Method of defining a UAV flight path using 3D parametric geometry |
| CN103365299A (en) * | 2013-08-02 | 2013-10-23 | 中国科学院自动化研究所 | Method and device for avoiding obstacle of unmanned aerial vehicle |
| CN109753068A (en) * | 2019-01-14 | 2019-05-14 | 哈尔滨工程大学 | A kind of more USV multi-agent synergy collision-avoidance planning methods considering signal intelligence |
| CN112130587A (en) * | 2020-09-30 | 2020-12-25 | 北京理工大学 | Multi-unmanned aerial vehicle cooperative tracking method for maneuvering target |
| CN113361152A (en) * | 2020-03-05 | 2021-09-07 | 北京京东乾石科技有限公司 | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus |
| CN113848985A (en) * | 2021-11-01 | 2021-12-28 | 西南交通大学 | Multi-carrier-based unmanned aerial vehicle path planning method based on Hybird A-star algorithm |
| CN114867988A (en) * | 2019-11-20 | 2022-08-05 | Wing航空有限责任公司 | Map comprising data for determining a route for an aircraft during a GNSS fault |
| CN114967744A (en) * | 2022-05-31 | 2022-08-30 | 哈尔滨工业大学 | Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance |
-
2023
- 2023-01-05 CN CN202310013673.XA patent/CN116257082B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103365299A (en) * | 2013-08-02 | 2013-10-23 | 中国科学院自动化研究所 | Method and device for avoiding obstacle of unmanned aerial vehicle |
| AU2013101142A4 (en) * | 2013-08-28 | 2013-10-03 | Parker, Scott Charles MR | Method of defining a UAV flight path using 3D parametric geometry |
| CN109753068A (en) * | 2019-01-14 | 2019-05-14 | 哈尔滨工程大学 | A kind of more USV multi-agent synergy collision-avoidance planning methods considering signal intelligence |
| CN114867988A (en) * | 2019-11-20 | 2022-08-05 | Wing航空有限责任公司 | Map comprising data for determining a route for an aircraft during a GNSS fault |
| CN113361152A (en) * | 2020-03-05 | 2021-09-07 | 北京京东乾石科技有限公司 | Trajectory planning method, trajectory planning device, storage medium, and electronic apparatus |
| CN112130587A (en) * | 2020-09-30 | 2020-12-25 | 北京理工大学 | Multi-unmanned aerial vehicle cooperative tracking method for maneuvering target |
| CN113848985A (en) * | 2021-11-01 | 2021-12-28 | 西南交通大学 | Multi-carrier-based unmanned aerial vehicle path planning method based on Hybird A-star algorithm |
| CN114967744A (en) * | 2022-05-31 | 2022-08-30 | 哈尔滨工业大学 | Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance |
Non-Patent Citations (2)
| Title |
|---|
| Formation Tracking and Obstacle Avoidance for Multiple Quadrotors With Static and Dynamic Obstacles;J. Qi;Robotics and Automation Letters;第7卷(第2期);第1713-1720页 * |
| 复杂环境下多无人机轨迹姿态 协同控制;田栢苓;航空学报;第41卷;第36-43页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116257082A (en) | 2023-06-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116257082B (en) | Distributed active cooperative detection method for multiple unmanned aerial vehicles | |
| CN102915652B (en) | aircraft traffic separation system and method | |
| Saunders et al. | Static and dynamic obstacle avoidance in miniature air vehicles | |
| Wen et al. | UAV online path planning algorithm in a low altitude dangerous environment | |
| Lawrance et al. | Path planning for autonomous soaring flight in dynamic wind fields | |
| CN112684807A (en) | Unmanned aerial vehicle cluster three-dimensional formation method | |
| Reinhart et al. | Learning-based path planning for autonomous exploration of subterranean environments | |
| CN110428111A (en) | Multi-Tasking method for planning track when UAV/UGV collaboration is long | |
| Wang et al. | Virtual reality technology of multi uavearthquake disaster path optimization | |
| CN105045274A (en) | Intelligent tower connected graph construction method for planning inspection route of unmanned aerial vehicle (UAV) | |
| CN114355981B (en) | Method and system for autonomous exploration and mapping of four-rotor unmanned aerial vehicle | |
| Liu | A progressive motion-planning algorithm and traffic flow analysis for high-density 2D traffic | |
| Du | Multi-uav search and rescue with enhanced a algorithm path planning in 3d environment | |
| CN115657721A (en) | Space environment unmanned aerial vehicle trajectory planning method based on improved ant colony algorithm | |
| Niu et al. | UAV track planning based on evolution algorithm in embedded system | |
| CN115060263A (en) | Flight path planning method considering low-altitude wind and energy consumption of unmanned aerial vehicle | |
| Qiao et al. | Multi-UAV cooperative patrol task planning novel method based on improved PFIH algorithm | |
| Cobano et al. | Thermal detection and generation of collision-free trajectories for cooperative soaring UAVs | |
| Zhao et al. | Autonomous Exploration Method for Fast Unknown Environment Mapping by Using UAV Equipped with Limited FOV Sensor | |
| Zhang et al. | UAV trajectory planning for complex open storage environments based on an improved RRT algorithm | |
| CN116518982B (en) | Low-altitude forestry monitoring remote sensing unmanned aerial vehicle path multi-target planning method | |
| CN116069023B (en) | Multi-unmanned vehicle formation control method and system based on deep reinforcement learning | |
| CN116804879A (en) | Robot path planning framework method for improving dung beetle algorithm and fusing DWA algorithm | |
| CN114596360B (en) | Double-stage active real-time positioning and mapping algorithm based on graph topology | |
| Zhang et al. | A State-Decomposition DDPG Algorithm for UAV Autonomous Navigation in 3D Complex Environments |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |