CN114967744A - Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance - Google Patents
Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance Download PDFInfo
- Publication number
- CN114967744A CN114967744A CN202210608189.7A CN202210608189A CN114967744A CN 114967744 A CN114967744 A CN 114967744A CN 202210608189 A CN202210608189 A CN 202210608189A CN 114967744 A CN114967744 A CN 114967744A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- track
- obstacle avoidance
- expected
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 68
- 230000006870 function Effects 0.000 claims abstract description 71
- 238000005457 optimization Methods 0.000 claims abstract description 37
- 230000008447 perception Effects 0.000 claims abstract description 25
- 238000011156 evaluation Methods 0.000 claims abstract description 23
- 230000001133 acceleration Effects 0.000 claims description 22
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000008569 process Effects 0.000 claims description 20
- 238000004891 communication Methods 0.000 claims description 14
- 230000004927 fusion Effects 0.000 claims description 13
- 238000005265 energy consumption Methods 0.000 claims description 11
- 230000004888 barrier function Effects 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 6
- 239000006185 dispersion Substances 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 2
- 230000007547 defect Effects 0.000 abstract description 2
- 238000001914 filtration Methods 0.000 abstract description 2
- 230000008901 benefit Effects 0.000 description 5
- 230000015572 biosynthetic process Effects 0.000 description 5
- 239000000243 solution Substances 0.000 description 5
- 230000003993 interaction Effects 0.000 description 4
- 239000002245 particle Substances 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000002347 injection Methods 0.000 description 2
- 239000007924 injection Substances 0.000 description 2
- ALZOLUNSQWINIR-UHFFFAOYSA-N quinmerac Chemical compound OC(=O)C1=C(Cl)C=CC2=CC(C)=CN=C21 ALZOLUNSQWINIR-UHFFFAOYSA-N 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000011218 segmentation Effects 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000009966 trimming Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
A planning method for multi-unmanned aerial vehicle collaborative obstacle avoidance relates to the technical field of unmanned aerial vehicle planning, and aims to solve the problem that multi-unmanned aerial vehicle cannot efficiently complete collaborative tasks in unknown complex environments in the prior art. The evaluation function set of the grids is designed based on Fast Marching algorithm, so that the conduction of analog waves at unnecessary grids is reduced, the searching speed of a global path can be increased, and the planning efficiency is improved; and only local track is optimized, the smoothness of the unmanned aerial vehicle motion is guaranteed, an additional smooth filtering algorithm is not needed, the calculated amount is greatly reduced compared with the global track optimization, the time cost can be saved to a great extent, and the real-time online planning requirement can be met. The method combines the consistency of perception information and the guidance of the cooperative structure, overcomes the defects of insufficient flexibility and adaptability and the like of the multi-unmanned aerial vehicle cooperative obstacle avoidance planning method in the unknown complex environment, and accordingly effectively improves the execution efficiency of the multi-unmanned aerial vehicle cooperative task in the unknown environment.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicle planning, in particular to a planning method for multi-unmanned aerial vehicle collaborative obstacle avoidance.
Background
Along with the continuous extension of unmanned aerial vehicle application in numerous fields, the degree of difficulty of carrying out the task also is constantly promoting, and single unmanned aerial vehicle operation has can't satisfy complicated task demand, and many unmanned aerial vehicles cooperate the task mode by extensive recognition and application. Under the scene of multi-unmanned aerial vehicle task cooperation, a distributed structure is usually adopted among the multi-unmanned aerial vehicles and a cooperative structure, also called a task formation, is used for maximizing the execution efficiency and flexibility of tasks.
When multiple unmanned aerial vehicles execute a cooperative task in an unknown complex environment, the conflict problems of emergent obstacle avoidance and cooperative structure maintenance are inevitably involved. How to effectively utilize perception information to plan smooth collaborative obstacle avoidance tracks for multiple unmanned aerial vehicles is of great importance, and on the premise of meeting various constraint relations and flight safety of each unmanned aerial vehicle, collaborative structures are attached as far as possible to execute unmanned aerial vehicle collaborative tasks. The method for planning the multi-unmanned aerial vehicle collaborative obstacle avoidance is a precondition for smoothly and efficiently executing the multi-unmanned aerial vehicle collaborative task. Therefore, the research on the collaborative obstacle avoidance planning among the multiple unmanned aerial vehicles has a far-reaching application value.
In the aspect of obstacle avoidance by cooperation of multiple unmanned aerial vehicles, the existing means mostly researches the gravity center on formation strategy, cooperative control and single-machine obstacle avoidance planning, changes the formation form to avoid the obstacle when encountering the obstacle, cannot achieve cooperative obstacle avoidance on the basis of a cooperative mechanism, and simultaneously ignores the sharing and fusion problems of sensing information and flight information of the multiple unmanned aerial vehicles in the obstacle avoidance planning process. Therefore, the traditional formation obstacle avoidance algorithm cannot decide the optimal cooperative obstacle avoidance planning action according to the consistent perception information and the cooperative structure.
The patent publication CN113442140A proposes a Cartesian space obstacle avoidance planning method based on Bezier optimization, which adopts a four-order Bezier curve as a target track model for algorithm optimization, determines three control point positions in the middle to determine the track shape, adopts a particle swarm algorithm to iteratively optimize the three middle control point positions, utilizes the four-order Bezier curve to calculate and obtain an obstacle avoidance track equation, and combines an actual control sampling period to complete obstacle avoidance track planning. However, in the method, the starting point track and the end point track are optimized into a Bezier track with only five key points, the position of three intermediate control points is iteratively optimized by adopting the particle swarm algorithm to adjust the shape of the curve, the iteration depth of the particle swarm algorithm is also increased even if no solution exists in the complex environment, and the method has poor adaptability. However, the problem of large iteration depth can be better solved by segmenting the track between the starting point and the end point.
Therefore, the consistency of perception information and the guidance of the cooperative structure are combined, the multi-unmanned aerial vehicle cooperative obstacle avoidance planning method with high flexibility and adaptability is designed, and the method has important significance for efficiently completing cooperative tasks in unknown complex environments by multiple unmanned aerial vehicles.
Disclosure of Invention
The purpose of the invention is: aiming at the problem that multiple unmanned aerial vehicles cannot efficiently complete cooperative tasks in an unknown complex environment in the prior art, a planning method for cooperative obstacle avoidance of multiple unmanned aerial vehicles is provided.
The technical scheme adopted by the invention to solve the technical problems is as follows:
a planning method for multi-unmanned aerial vehicle collaborative obstacle avoidance comprises the following steps:
the method comprises the following steps: setting task scene parameters and task parameters;
the task scene parameters include: unmanned aerial vehicle number N with same configuration and size of task environment scene uav And a grid dispersion resolution;
the task parameters comprise: starting point coordinates start _ pt and end point coordinates end _ pt of the collaborative obstacle avoidance planning and the detectable distance of the unmanned aerial vehicle, wherein the detectable distance of the unmanned aerial vehicle is sense _ horizon;
step two: the unmanned aerial vehicle carries out isomorphic information exchange with an adjacent unmanned aerial vehicle through an unmanned aerial vehicle communication neighbor table;
the isomorphic information comprises perception information and flight state information;
the perception information comprises coordinates of the current position of the unmanned aerial vehicle in a world coordinate system and a local occupation grid map held by the unmanned aerial vehicle;
the flight state information comprises the global path and the expected track of the unmanned aerial vehicle;
the local occupancy grid map construction steps are as follows:
firstly, discretizing the size of a task environment scene by using grid dispersion resolution to obtain the size of a local occupied grid map, then continuously acquiring the position and the geometric property of an obstacle in a self-sensing range by using an unmanned aerial vehicle through an airborne sensor in the flight process, performing convex expansion processing on the position and the geometric property of the obstacle to obtain an obstacle grid area, and finally constructing the local occupied grid map according to the size of the local occupied grid map and the obstacle grid area;
step three: designing a distributed information fusion algorithm based on a maximum consistency theory and a distributed communication strategy, then carrying out iterative updating on a local occupancy grid map held by a current unmanned aerial vehicle by using the distributed information fusion algorithm and perception information exchanged by the current unmanned aerial vehicle and other unmanned aerial vehicles, setting an identification cache pool, stopping the iteration when the information in the identification cache pool is not updated, and at the moment, obtaining the local occupancy grid map held by the unmanned aerial vehicle as an optimal global occupancy grid map G;
step four: searching an unobstructed time shortest global path between a start coordinate start _ pt and an end coordinate end _ pt of the collaborative obstacle avoidance plan;
step five: selecting a local target point tag _ pt in a sense _ horizon range by using an unobstructed time shortest global path, and performing track optimization on a path between a starting point coordinate start _ pt and the local target point tag _ pt of the collaborative obstacle avoidance plan to obtain a smooth feasible expected track leading from the starting point coordinate start _ pt to the local target point tag _ pt;
step six: and D, performing collision detection on the smooth feasible expected track obtained in the step five by using the optimal global occupancy grid map G obtained in the step three, wherein the collision detection specifically comprises the following steps:
setting forward inspection time t _ head and emergency braking time t _ stop, and then calculating collision probability p _ collision, wherein the collision probability p _ collision is expressed as:
dv is the speed of a newly detected obstacle, dp is the current position of the obstacle, odomPt is the current position of the unmanned aerial vehicle, getOccupy is an occupation probability acquisition function of an optimal global occupation grid map G, and prePt is an expected track point of the unmanned aerial vehicle at the moment t;
if the collision probability p _ collision is larger than 0.5, judging that the smooth feasible expected track and the newly detected obstacle generate collision conflict;
setting a collision index as 1 when the collision and collision opportunity is within the forward check time t _ head; when the collision conflict opportunity is within the braking time t _ stop, the unmanned aerial vehicle enters an emergency hovering state;
step seven: when the re-planning time is reached, updating the starting point start _ pt of the collaborative obstacle avoidance planning to be an expected track point advanced by 0.5 second at the current moment of the unmanned aerial vehicle, and re-executing the fourth step, the fifth step and the sixth step;
the replanning time is as follows:
(1) the Euclidean distance between the tag _ pt of the local target point and the current position odompT of the unmanned aerial vehicle reaches a set threshold, and the unmanned aerial vehicle does not reach the terminal point;
(2) the collision index value obtained in the fifth step is equal to 1;
(3) the distance between the current unmanned aerial vehicle and the neighbor unmanned aerial vehicle is larger than the cooperative distance or smaller than the safe distance, and the distance between the current unmanned aerial vehicle and the neighbor unmanned aerial vehicle is obtained through flight state information exchanged between the current unmanned aerial vehicle and other unmanned aerial vehicles.
Further, the barrier-free shortest-time global path in step four is obtained by the following steps:
the method comprises the steps of simulating wave propagation in an environment by utilizing a local occupied grid map of a current unmanned aerial vehicle and taking a start coordinate start _ pt as a wave source, evaluating adjacent nodes of current propagation environment nodes in the propagation process to obtain evaluation values, selecting the adjacent node with the minimum evaluation value as a next propagation environment node, finishing the propagation process when the wave is expanded to an end coordinate end _ pt, tracing back the propagation path at the end coordinate end _ pt along the gradient descending direction of arrival time to obtain the propagation path with the shortest time from the end coordinate end _ pt to the start coordinate start _ pt, namely the barrier-free shortest time global path.
Further, the evaluation value is obtained through an evaluation function f (n) of the current propagation environment node, and the evaluation function f (n) includes a time cost function t (n), a heuristic information function h (n), and a collaborative obstacle avoidance benefit function e (n);
the evaluation function f (n) is expressed as:
f(n)＝γ t T(n)+γ h H(n)-γ e E(n)
wherein, γ t ,γ h ,γ e The time cost function T (n), the heuristic information function H (n) and the cooperative obstacle avoidance profit function E (n) are respectively the proportionality coefficients in the evaluation function f (n).
Further, the time cost function t (n) is expressed as:
wherein, T x ,T y ,T z The minimum of the arrival times of all neighbors of grid n in x, y, z directions, respectively, and T x ＞T y ＞T z H is the spacing of adjacent grids and F is the propagation velocity of the wave at grid n.
The F is obtained through a speed field function, and the speed field function is established according to a local occupation grid map and by utilizing an ESDF map;
the velocity field function is expressed as:
the heuristic information function H (n) is expressed as:
the collaborative obstacle avoidance revenue function e (n) is expressed as:
wherein v is max D is the signed Euclidean distance of the current grid n from the nearest obstacle obtained from the ESDF map, e is the Euler constant of about 2.71828,
is a hyperbolic tangent function; dx, dy and dz are the distance difference between the grid n and the end point coordinate end _ pt in the x, y and z directions, a ij For the communication relationship between drone i and drone j, d ij The deviation of the reference virtual center corresponds to drone i and drone j.
Further, the concrete steps of the fifth step are as follows:
step five, first: establishing an expected track model of the unmanned aerial vehicle based on the piecewise continuous eight-order Bezier curve;
step five two: taking a set of control points in an expected trajectory model of the unmanned aerial vehicle as an optimized object, solving a minimization problem, namely a trajectory optimization problem, of a flight energy consumption cost Je and a coordinated structure consistent cost Jc in a barrier-free safety convex set, solving according to the trajectory optimization problem to obtain a value of the optimized object, and finally bringing the value of the optimized object into the expected trajectory model to obtain a smooth feasible expected trajectory leading from a start coordinate start _ pt to a local target point tag _ pt.
Further, the expected trajectory model of the drone is represented as:
wherein m is the number of track segments in the expected track model, n is the order of the Bezier curve in the expected track model, i is the upper label and
is an n-order Bezier curve basis function, i.e. Bernstein polynomial, p 1,i 、p 2,i …p m,i Segmenting track sections in an expected track model, wherein the track sections are n-order Bezier curve control points corresponding to the tracks with the labels of 1, 2. T is a time parameter, T 0 、T m Set respective start and end times, T, for the expected trajectory 0 、T 1 …T m-1 Respectively, a track start time, s, which is denoted by track segment reference number 1,2 1 、s 2 …s m Scaling a trajectory time interval scale factor, labeled 1, 2.. eta.m, for a trajectory segment in the expected trajectory model, for scaling a time parameter T from a time interval [ T [ ] m-1 ,T m ]Scaling to fixed time interval [0,1 ]]。
Further, the optimization object is represented as:
wherein p is 1,0 …p 1,8 …p m,0 …p m,8 The control point p of the Bezier curve of the order n corresponding to the track with the mark number of 1,2 1,i 、p 2,i …p m,i Taking n as 8, i belongs to [0, n ]]The detailed description of (1); x is the number of 1,0 …x 1,8 …x m,0 …x m,8 、y 1,0 …y 1,8 …y m,0 …y m,8 、z 1,0 …z 1,8 …z m,0 …z m,8 Respectively, the track segments in the expected track model are labeled as 1,2, 1,i 、p 2,i …p m,i taking n as 8, i belongs to [0, n ]]Expanded representation in three dimensions x, y, z.
Further, the trajectory optimization problem is expressed as:
min J＝λ e J e +λ c J c
wherein J is an objective function of the trajectory optimization problem, J e As a flight energy consumption cost function, J c For co-structural uniform cost functions, λ e 、λ c Is a proportionality coefficient, N is the number of neighboring drones of the current drone i, j is the number of neighboring drones, aij represents the communication relationship between drone i and drone j,
corresponding to the reference virtual center positions, f, for UAV i and UAV j, respectively i (t) position of drone i at time t, O i Is the reference virtual center vector for drone i.
Further, the specific step of obtaining a smooth feasible prospective track leading from the start coordinate start _ pt to the local target point tag _ pt is as follows:
step 1: intercepting a path between a starting point coordinate start _ pt and a local target point tag _ pt from the barrier-free time shortest global path obtained in the fourth step, expanding each grid in the path into a largest cube in a safe space according to the grid in which the grid is positioned, and finishing expansion when the boundary of the cube touches an barrier or the expansion times reach safe _ ite;
taking the number of the cubes after the largest cubes in the safety space are simplified as the number m of the sections of the track in the expected track model;
setting a track segment starting point state for the 1 st track in the segment number m of the track, wherein the starting point state comprises a position start _ pt, a speed start _ vel and an acceleration start _ acc at the starting point of the unmanned aerial vehicle, and is represented as follows:
p 1,0 ·s 1 ＝start_pt
8(p 1,1 -p 1,0 )＝start_vel
setting a track segment end point state for the mth track in the segment number m of the track, wherein the end point state corresponds to the local target point position tag _ pt, and the speed and the acceleration are set to be zero, and are expressed as follows:
p m,5 ·s m ＝tag_pt
8(p m,6 -p m,5 )＝[0,0,0]
step 2: adding a maximum speed limit, an acceleration limit and a continuity limit between the segmented tracks to the expected track model; the maximum speed limit is:
-v max ≤n·(p j,i -p j,i-1 )≤v max ,i＝1,2,...,n
the acceleration limit is expressed as:
the inter-segment trajectory continuity constraint is expressed as:
s j ·p j,n -s j+1 ·p j+1,0 ＝0
(p j,n -p j,n-1 )-(p j+1,1 -p j+1,0 )＝0
and step 3: taking the simplified cube as a domain, integrating the contents in the step 1 and the step 2 to obtain a quadratic programming problem, wherein the solution of the quadratic programming problem is the position of a control point in an expected trajectory model of the unmanned aerial vehicle, and further obtaining a smooth feasible expected trajectory leading to a local target point;
wherein n is the order of the Bezier curve in the expected track model, v max 、a max Maximum velocity and acceleration, p, respectively j,i-2 、p j,i-1 、p j,i 、p j,n-2 、p j,n-1 、p j,n Control points p with the labels i-2, i-1, i, n-2 and n in the n-order Bezier curve corresponding to the jth track in the number m of the track sections j+1,0 、p j+1,1 、p j+1,2 Control points with the upper labels of 0,1 and 2 in an n-step Bezier curve corresponding to the j +1 th track in the track segment number m respectively, s j 、s j+1 The track time interval scaling factors for the j-th and j + 1-th segments, respectively, in the number of segments m of the track.
Further, the quadratic programming problem is expressed as:
min p T Qp+C T p+c f
s.t.p j ∈Ω j ,j＝1,2,...,m
A eq p＝b eq
l ieq ≤A ieq p≤u ieq
wherein, the number of segments of the m tracks, Q is the Hessian matrix of the objective function J, C is the linear coefficient matrix of the objective function J, and C f A constant term of the objective function J, p is an n-order Bezier curve control point p corresponding to a track with a track segment label of 1,2 1,i 、p 2,i …p m,i Is represented by a matrix of，p j Is a matrix representation of the control points of the Bezier curve of the n-th order corresponding to the jth track in the number m of the sections of the track, omega j Is a matrix p j Of barrier-free safety convex sets, i.e. matrices p j Definition domain of A eq 、b eq Coefficient matrix and constant matrix constrained by equation of objective function J, A ieq Coefficient matrix constrained by inequality of objective function J,/ ieq 、u ieq A corresponding minimum constant matrix and maximum constant matrix.
The invention has the beneficial effects that:
the distributed structure that this application adopted designs distributed information fusion algorithm through neighbouring unmanned aerial vehicle's information interaction, can make all unmanned aerial vehicles obtain the perception information of global uniformity in limited perception scope in the intercommunication network through finite iteration, can greatly improve the perception ability of each unmanned aerial vehicle to real-time situation, and the failure rate that helps unmanned aerial vehicle to accomplish cooperative task can effectively reduce cooperative task. Meanwhile, through interaction of flight information, the self planning state is adjusted in real time, the expected track is optimized, the cooperative structure can be attached to the maximum degree, and collision among multiple unmanned aerial vehicles is avoided. Compared with the traditional formation obstacle avoidance planning method, the synergy and the adaptability of the obstacle avoidance planning are enriched, and multiple nobody can make the optimal collaborative obstacle avoidance planning action.
The evaluation function set of the grid is designed based on the Fast Marching algorithm, so that the conduction of analog waves at unnecessary grids is reduced, the searching speed of a global path can be accelerated, and the planning efficiency is improved; and only local track is optimized, the smoothness of the unmanned aerial vehicle motion is guaranteed, an additional smooth filtering algorithm is not needed, the calculated amount is greatly reduced compared with the global track optimization, the time cost can be saved to a great extent, and the real-time online planning requirement can be met.
The method considers the interference of the static barrier and the dynamic barrier simultaneously, and improves the flight safety of the unmanned aerial vehicle in the unknown complex environment.
The method combines the consistency of perception information and the guidance of the cooperative structure, overcomes the defects of insufficient flexibility and adaptability and the like of the multi-unmanned aerial vehicle cooperative obstacle avoidance planning method in the unknown complex environment, and accordingly effectively improves the execution efficiency of the multi-unmanned aerial vehicle cooperative task in the unknown environment.
Drawings
Fig. 1 is a flow chart of planning of cooperative obstacle avoidance by multiple drones according to the present application;
FIG. 2 is a pseudo-code diagram of a distributed information fusion algorithm according to the present application;
fig. 3 is a schematic diagram of a global path search result of multi-drone collaborative obstacle avoidance;
fig. 4 is a schematic diagram of a local trajectory optimization result of multi-unmanned aerial vehicle collaborative obstacle avoidance.
Detailed Description
It should be noted that, in the present invention, the embodiments disclosed in the present application may be combined with each other without conflict.
The first embodiment is as follows: specifically describing the present embodiment with reference to fig. 1, the planning method for collaborative obstacle avoidance by multiple unmanned aerial vehicles in the present embodiment includes:
step 1: the parameter set is initialized. The method comprises task parameters, a cooperative structure, a cooperative obstacle avoidance planning algorithm and other related parameters.
Step 2: and (4) environment perception and fusion. The unmanned aerial vehicle sets up to the non-obstacle region of ideal to unknown environment, and unmanned aerial vehicle constantly obtains information such as obstacle position and geometric properties of self perception scope through airborne sensor at the flight in-process, and is right the obstacle is made the protruding inflation and is handled, builds the local grid map that occupies.
The aircraft with the same configuration continuously exchanges isomorphic information with the adjacent unmanned aerial vehicle at a certain frequency, wherein the isomorphic information comprises perception information and flight state information. The perception information comprises coordinates of the current position of the unmanned aerial vehicle in a world coordinate system and a local occupation grid map held by the unmanned aerial vehicle; the flight state information comprises a global path and an expected track of the unmanned aerial vehicle obtained through a collaborative obstacle avoidance planning algorithm.
A distributed information fusion algorithm is designed based on a maximum consistency theory and a distributed communication strategy, and all unmanned aerial vehicles in a connected network can obtain globally consistent perception information in a limited perception range through limited iteration. In addition, through interaction with the flight information of the adjacent aircraft, a decision basis is provided for collaborative obstacle avoidance planning, the planning state of the adjacent aircraft is adjusted in real time, and the expected track is optimized.
And step 3: and searching a global path. Determining a starting point coordinate and an end point coordinate of the collaborative obstacle avoidance plan, constructing an incremental ESDF map by using a tool FIESTA, and establishing a reasonable speed field for a task area according to the ESDF map. An evaluation function set { time cost T (n), heuristic information H (n) and collaborative obstacle avoidance benefits E (n) } of the current grid n is designed based on a Fast Marching algorithm, the searching speed of the global path is increased, and an optimal global path without obstacles is searched between a starting point and an end point.
The search for an optimal global path without obstacles is performed by the Fast Marching (FM) algorithm, which is a method for solving equation of function first proposed by Sethian
The numerical method is a level set method for solving the wave propagation problem in the field of image processing. The method represents a mathematical model of wave propagation as an Equation of an Equation
Where F is the wave propagation speed and T is the arrival time of a certain environmental node. The algorithm efficiently solves the equation of the equation by first order numerical approximation and estimates using the windward strategy
I.e. the wave will only propagate from a position where the value of T is small to a direction where the value of T is larger.
At present, the FM algorithm is gradually applied to the field of path planning of mobile robots, and the shortest path from a starting point to a target point of an unmanned aerial vehicle can be regarded as the shortest time path from a wave source to the target point. The path searching process is equivalent to the forward propagation process of the wave, in the propagation process, the time required by the wave to reach the adjacent node is calculated by utilizing the approximation of a first-order numerical value of the function equation and is used as an evaluation value of the adjacent node, and the adjacent node with the minimum evaluation value is selected as the expansion direction of the wave.
In addition, by setting a reasonable propagation speed F, the path points searched by the FM algorithm can be far away from the surface of the obstacle and the narrow space in the environment as far as possible, and the path points are in a wider space, so that the attitude change rate in the flying process of the unmanned aerial vehicle is reduced, and the energy consumption and the flying difficulty are reduced.
Wherein the time cost T (n) describes the shortest arrival time of the grid n, which can be obtained by solving the Eikonal equation, so that the path is more inclined to select a sparse barrier environment to avoid frequently changing the flight attitude; heuristic information H (n) describes the optimal path estimation cost from the grid n to the end point, so that the search process is more prone to expand towards the end point; the collaborative obstacle avoidance benefit e (n) describes that in the global path search process, the path should be located in an area where the cumulative reference virtual center deviation of the multiple drones is small as much as possible, and guides the global path of each drone to fit the collaborative structure as much as possible.
And 4, step 4: and optimizing a local track. And (4) selecting a local target point tag _ pt in the observable range according to the optimal global path obtained in the step (3), and performing trajectory optimization only on the path between the starting point start _ pt and the local target point tag _ pt of the unmanned aerial vehicle to generate a smooth feasible expected trajectory leading to the local target point to guide flight. The method specifically comprises the following steps:
step 4.1: an expected trajectory model of the drone is determined. By adopting the eight-order Bezier curve with continuous segmentation, the sudden change of the position, the speed, the acceleration and the angular velocity at the connecting point of each segment of track can be avoided, and the flexibility of the segmented track also ensures that most feasible tracks are covered. The expected trajectory model expression is as follows:
the number of segments of the expected track model is m, the order n is 8, and the superscript i belongs to [0, n ∈ n]。
Is a collection of expected trajectory model control points,
is a Bernstein polynomial. [ T ] 1 ,T 2 ,...,T m ]Representing the pre-allocated end time for each track segment, and the total time is T ═ T m -T 0 ，[s 1 ,s 2 ,...,s m ]Representing a time scale factor for varying the time parameter T from T m-1 ,T m ]Scaling to fixed time interval [0,1 ] of Bezier curve]And the track optimization problem is more stable.
Step 4.2: and determining an optimization problem model. Defining an objective function of the optimization problem as solving a flight energy consumption cost J in a barrier-free safety convex set e Cost consistent with cooperative structure J c The minimization problem of (2). The model expression for the optimization problem is as follows:
min J＝λ e J e +λ c J c
wherein λ e ,λ c As a proportionality factor, flight energy consumption cost J e The integral describing the square of the fourth derivative of the trajectory corresponds directly to the flight energy consumption. Coordinated structure consistent cost J c And describing a two-norm deviation of the current position and the expected position deviation of the cooperative structure, wherein the larger the position deviation is, the larger the cost for achieving the consistency of the cooperative structure is.
Step 4.3: a constraint is determined. Maximum speed and acceleration limits are added to the expected trajectory, as well as the continuity limit between the segmented trajectories. And the continuity limitation among the segmented tracks describes that the position, the speed and the acceleration of each two segments of tracks are equal at the track connecting point.
Step 4.4: and optimizing object injection. And selecting the local target point as an intersection point of the global path and the observable range, and setting the starting point start _ pt and the position tag _ pt of the local target point of the unmanned aerial vehicle as the starting point and the target point of the local track optimization problem.
Step 4.5: and (5) solving an optimization problem. Since the trajectory optimization problem is a linear constraint quadratic minimization problem and the domain is defined as a convex set, the trajectory optimization problem can be converted into solving a Quadratic Programming (QP) problem.
And 5: and (4) collision detection. Because the environment perceived by the unmanned aerial vehicle itself changes constantly, it is necessary to perform forward security check on the expected trajectory from the current position odomPt of the unmanned aerial vehicle in time at the trajectory execution stage.
And setting forward check time t _ ahead, emergency braking time t _ stop, collision probability p _ collision and a collision index with a default value of 0. Setting a collision index as 1 when the expected track has the existing collision conflict within the forward check time t _ head; and when the collision conflict opportunity is within the braking time t _ stop, simultaneously enabling the unmanned aerial vehicle to enter an emergency hovering state.
Whether the expected track point prePt collides with a newly detected obstacle at the time t is judged according to whether the collision probability p _ collision is greater than 0.5, wherein the collision probability p _ collision specifically comprises the following steps:
and 6: and (4) performing online re-planning. And determining the re-planning time, updating the expected track point of the unmanned aerial vehicle which leads the current time by a period of time t _ duration from the starting point start _ pt of the collaborative obstacle avoidance planning, and re-executing the steps 3, 4 and 5. The timing of the replanning includes the following:
(1) the euclidean distance of the local target point to the location of the drone is close to a certain threshold and the drone is not close to the end point.
(2) The value of the collision index obtained in step 5 is equal to 1.
(3) The distance between the unmanned aerial vehicle and the neighboring unmanned aerial vehicle is greater than the cooperative distance or less than the safe distance.
Example (b):
step 1: the parameter set is initialized. Mainly comprises
Setting a task Environment scene size S x ×S y ×S z Number of obstacles N obstacles Number of unmanned aerial vehicles N of the same configuration uav Grid dispersion resolution, cooperative structure and other related parameters, and unmanned aerial vehicle communication neighborhoodAnd the parameters related to distributed information fusion algorithms such as the table, the maximum iteration times env _ ite and the like, and the parameters related to global path search and local track optimization such as the start coordinate start _ pt and the end coordinate end _ pt of the collaborative obstacle avoidance plan, the maximum expansion times safe _ ite of the safe flight area and the like.
The method comprises the steps that a task environment area is set to be S, the sensor field angle of the unmanned aerial vehicle is 120 degrees, the detectable distance is sense _ horizon, and the unmanned aerial vehicle divides the task area S into a three-dimensional occupation grid map according to the dispersion resolution. Setting the maximum iteration times env _ ite as the maximum diameter N of the connected subgraph uav -1, wherein N uav The number of unmanned aerial vehicles participating in the collaborative obstacle avoidance planning.
The invention sets the plane geometric center of the cooperative structure of multiple unmanned aerial vehicles as a virtual center based on a virtual structure method, and presets a virtual center reference vector O according to the relative position of the virtual center and each unmanned aerial vehicle k 。
The unmanned aerial vehicle continuously obtains the position and the geometric property of an obstacle in a self-sensing range through an airborne sensor in the flying process, performs convex expansion processing by utilizing the position and the geometric property of the obstacle, and constructs a local occupied grid map;
step 2: and (4) environment perception and fusion. Considering that the task environment is globally unknown and the attitude of the unmanned aerial vehicle is dynamically changed, the unmanned aerial vehicle continuously detects the task environment covered in the range of a sensor of the unmanned aerial vehicle during the flight process, expands the detected obstacles into a cube model, updates GridMap by adopting an occupation probability method, and constructs a local occupation grid map Collision _ localMap. The probability of occupation of the grid involved during the expansion of the obstacle is set to 1.
The airplane with the same configuration continuously exchanges isomorphic information with the adjacent unmanned aerial vehicle, and the isomorphic information comprises two parts. Respectively as follows:
(1) perception information: the coordinates of the current position of the unmanned aerial vehicle in the world coordinate system and the self-held local occupancy grid map Collision _ localMap are included.
(2) Flight state information: the unmanned aerial vehicle obstacle avoidance method comprises a self global path and an expected track which are obtained by the unmanned aerial vehicle through a collaborative obstacle avoidance planning algorithm.
And designing a distributed information fusion algorithm based on the maximum consistency theory and the distributed communication strategy, as shown in fig. 2. At most, all unmanned aerial vehicles in the connected network can obtain globally consistent perception information in a limited perception range through env _ ite iterations. The global consistency judging method is defined as that no update exists in the identification cache pool, specifically, when each unmanned aerial vehicle is communicated with an adjacent machine, cache pool information is exchanged simultaneously, other unmanned aerial vehicle identifications which are directly or indirectly communicated with the identification cache pool are added in the identification cache pool, and when the information in the cache pool is not updated, the global consistency is considered to be achieved.
When the kth iteration is carried out, the unmanned aerial vehicle shares isomorphic information to adjacent machines in a communication range, and meanwhile, the local occupancy grid map Collision _ localMap of the unmanned aerial vehicle is updated through perception information shared by the adjacent machines based on the maximum consistency protocol. The update rule of the maximum consistency protocol is as follows:
in addition, through interaction with the flight information of the adjacent aircraft, a decision basis is provided for collaborative obstacle avoidance planning, the planning state of the adjacent aircraft is adjusted in real time, and the expected track is optimized.
And 3, step 3: and searching a global path. Determining a start coordinate start _ pt and an end coordinate end _ pt of the collaborative obstacle avoidance planning, designing an evaluation function set { time cost T (n), heuristic information H (n) and collaborative obstacle avoidance profit E (n) } of a current node n based on a Fast Marching algorithm, and searching for a shortest global path without obstacles between the start point and the end point. The method specifically comprises the following steps:
before searching a global path by using an improved Fast Marching algorithm, an incremental ESDF map is constructed by using a tool FIESTA according to a local three-dimensional occupied grid map Collision _ localMap, so that distance and gradient information query can be conveniently carried out on obstacles in a sensing range.
And establishing a reasonable speed field aiming at the flight environment, so that the set flight speed of each grid in the grid map GridMap is in direct proportion to the distance between the grid and the nearest obstacle. The velocity field function is established from the ESDF map as follows:
where d is the signed Euclidean distance from the current point to the nearest barrier as derived from the barrier ESDF map, e is the Euler constant of about 2.71828, and v is max Is the maximum flight speed of the quad-rotor unmanned aerial vehicle,
is a hyperbolic tangent function.
And starting from a starting point to simulate wave conduction, designing an evaluation function set of the current grid n, reducing the conduction of the simulated wave at an unnecessary grid, and accelerating the search speed of a global path. The evaluation function expression for grid n is as follows:
f(n)＝γ t T(n)+γ h H(n)-γ e E(n)
the evaluation function set is composed of three parts which are respectively:
(1) time cost T (n): the time cost t (n) describes the shortest arrival time of grid n, obtained by solving Eikonal's equation, making the search process more prone to sparse obstacle environments to avoid frequent changes in flight attitude. The expression is as follows:
where T is the time of arrival to be achieved for the current grid n, T x ,T y ,T z Is the minimum of the arrival times of all neighbors of grid n in the x, y, z directions, respectively, and T x ＞T y ＞T z And h is the spacing of adjacent grids.
(2) Heuristic information H (n): heuristic information h (n) describes the best path estimation cost to reach the end point from grid n, making the search process more prone to spread towards the end point. The expression is as follows:
where dx, dy, and dz are the differences between the grid n and the end point in the x, y, and z directions, respectively.
(3) Collaborative obstacle avoidance benefit E (n): in the global path searching process, the paths should be located in the area with small deviation of the accumulated reference virtual centers of the multiple unmanned aerial vehicles as much as possible, and the global paths of the unmanned aerial vehicles are guided to be attached to the cooperative structure as much as possible. The smaller the deviation of the corresponding accumulated reference virtual center in the unmanned aerial vehicle selection search direction is, the more complete the cooperative structure is, and the larger the cooperative obstacle avoidance benefit is. The expression is as follows:
wherein a is ij Representing the communication relationship between drone i and drone j, d ij The reference virtual center deviation is corresponded to both.
When the grid point n is the end point, the search process is ended, and the path is traced back at the end point along the gradient descending direction of the arrival time, so that a shortest time path from the end point to the start point is obtained, as shown in fig. 3.
And 4, step 4: and optimizing a local track. And (4) selecting a local target point tag _ pt in the observable range according to the optimal global path obtained in the step (3), and performing trajectory optimization only on the path between the starting point start _ pt and the local target point tag _ pt of the unmanned aerial vehicle to generate a smooth feasible expected trajectory leading to the local target point to guide flight. The method specifically comprises the following steps:
step 4.1: an expected trajectory model of the drone is determined. By adopting the eight-order Bezier curve with continuous segmentation, the sudden change of the position, the speed, the acceleration and the angular velocity at the connecting point of each segment of track can be avoided, and the flexibility of the segmented track also ensures that most feasible tracks are covered. The expected trajectory model expression is as follows:
the number of segments of the expected track model is m, the order n is 8, and the superscript i belongs to [0, n ∈ n]。[T 1 ,T 2 ,...,T m ]Representing the pre-allocated end time for each track segment, and the total time is T ═ T m -T 0 ，[s 1 ,s 2 ,...,s m ]Representing a time scale factor for varying the time parameter T from T m-1 ,T m ]Scaling to fixed time interval [0,1 ] of Bezier curve]And the track optimization problem is more stable.
In the expected trajectory model [ p ] 1,i ,p 2,i ,...,p m,i ]The method is a set of m sections of continuous eight-order Bezier curve control points, each section of track adopts 8 control points to correspond to the state of the unmanned aerial vehicle, and the state comprises the position, the speed, the acceleration and the angular speed of the unmanned aerial vehicle. Wherein the control point [ p ] l,0 ,p l,1 ,p l,2 ,p l,3 ],l∈[1,m]Representing the starting state of the track segment, control point [ p ] l,5 ,p l,6 ,p l,7 ,p l,8 ],l∈[1,m]And ensuring continuous and smooth tracks of all the sections corresponding to the end point state of the track section.
Bernstein polynomial
Is an nth order Bezier curve basis function,
the expression of (a) is as follows:
base functions of expected trajectory model
Conversion to power base t i Then, the expected trajectory model correspondence matrix representation when m is 1 is specifically:
step 4.2: and determining an optimization problem model.
Determining a control point position [ p ] in an expected trajectory model of an unmanned aerial vehicle 1,i ,p 2,i ,...,p m,i ]To determine the shape of the segmented trajectory, since the expected trajectory model is an m-segment continuous eight-order Bezier curve and the control points are all three-dimensional coordinates, the overall optimization object is a (8+1) · m × 3-dimensional particle, specifically:
defining an objective function of the optimization problem as solving a flight energy consumption cost J in a barrier-free safety convex set e Cost J consistent with the cooperative structure c The minimization problem of (2). The model expression for the optimization problem is as follows:
min J＝λ e J e +λ c J c
flight energy consumption cost J e : describing the integral of the square of the fourth derivative of the track, according to the differential flatness characteristic of the quad-rotor unmanned aerial vehicle, the fourth derivative of the track corresponds to the angular acceleration and is related to the motor rotating speed to directly correspond to the flight energy consumption, and the expression is as follows:
coordinated structure consistent cost J c : and describing a two-norm deviation between the current position and the expected position of the cooperative structure, wherein the expected position of the cooperative structure is obtained by combining flight state information between adjacent unmanned aerial vehicles. When the deviation between the current position of each unmanned aerial vehicle and the expected position of the cooperative structure is smaller than a certain threshold value, the unmanned aerial vehicles can be considered to be attached to the cooperative structure at the moment, and the corresponding cooperative structures are consistent. The greater the positional deviation, the greater the cost of achieving a consistent cooperative structure. The expression is as follows:
wherein a is ij Representing the communication relationship between drone i and drone j,
and corresponding to the reference virtual centers for the unmanned aerial vehicle i and the unmanned aerial vehicle j respectively. The reference virtual center is described as the actual position of the unmanned aerial vehicle minus the reference position of the preset virtual center, and the expression is as follows:
step 4.3: a constraint is determined. Maximum speed and acceleration limits are added to the expected trajectory, as well as the continuity limit between the segmented trajectories.
Based on the convex hull nature of the Bezier curve and the nature of the curve shape as a function of control points, the maximum velocity and acceleration limits may be translated into corresponding constraints on the expected trajectory and the corresponding control points for the higher derivative of the trajectory. The constraint that the maximum speed and acceleration limit corresponds to on the jth track is expressed as:
-v max ≤n·(p j,i -p j,i-1 )≤v max ,i＝1,2,...,n
and the continuity limitation among the segmented tracks describes that the position, the speed and the acceleration of each two segments of tracks are equal at the track connecting point. The constraint that the continuity of each section of track is limited between the jth section of track and the jth +1 section of track is expressed as follows:
s j ·p j,n -s j+1 ·p j+1,0 ＝0
(p j,n -p j,n-1 )-(p j+1,1 -p j+1,0 )＝0
step 4.4: and optimizing object injection. And selecting the local target point as an intersection point of the global path and the observable range, and setting the starting point start _ pt and the position tag _ pt of the local target point of the unmanned aerial vehicle as the starting point and the target point of the local track optimization problem.
And expanding the path between the starting point and the local target point into a largest cube in the safety space according to the grid where the path is located, initializing the maximum expansion time to be safe _ ite, and finishing the expansion process in advance when the boundary of the cube touches an obstacle.
And trimming off repeated cubes and cubes contained in the path, and taking the number of simplified cubes as the number of sections of the track in the expected track model.
Since the cube is a typical convex hull structure, the piecewise trajectory optimization problem can be converted into a solution problem for single-piecewise trajectory control points within a single cube.
And calculating the path lengths corresponding to the two cubes, and pre-allocating flight time for the single-segment track by using the trapezoidal speed.
Setting track segment starting point states for the 1 st segment of track, wherein the starting point states comprise a position start _ pt, a speed start _ vel, an acceleration start _ acc and the like at the starting point of the unmanned aerial vehicle, and are expressed as follows:
p 1,0 ·s 1 ＝start_pt
8(p 1,1 -p 1,0 )＝start_vel
setting a track segment end point state for the mth track segment, wherein the end point state corresponds to a local target point position tag _ pt, and the speed and the acceleration are set to be zero and are represented as follows:
p m,5 ·s m ＝tag_pt
8(p m,6 -p m,5 )＝[0,0,0]
step 4.5: and (5) solving an optimization problem. Since the trajectory optimization problem is a linear constraint quadratic minimization problem and the domain is a convex set, it can be converted into solving a quadratic programming QP problem, which is expressed as follows.
min p T Qp+C T p+c f
s.t.p j ∈Ω j ,j＝1,2,...,m
A eq p＝b eq
l ieq ≤A ieq p≤u ieq
The solution to the above problem is the control point position [ p ] in the expected trajectory model of the drone 1,i ,p 2,i ,...,p m,i ]So that the multiple smooth tracks are spliced into an optimal track, as shown in fig. 4.
And 5: and (4) collision detection. Because the environment perceived by the unmanned aerial vehicle itself changes constantly, it is necessary to perform forward security check on the expected trajectory from the current position odomPt of the unmanned aerial vehicle in time at the trajectory execution stage.
And setting forward check time t _ ahead, emergency braking time t _ stop, collision probability p _ collision and a collision index with a default value of 0. Setting a collision index as 1 when the expected track has the existing collision conflict within the forward check time t _ head; and when the collision conflict opportunity is within the braking time t _ stop, simultaneously enabling the unmanned aerial vehicle to enter an emergency hovering state.
Whether the expected track point prePt collides with a newly detected obstacle at the time t is judged according to whether the collision probability p _ collision is greater than 0.5, wherein the collision probability p _ collision specifically comprises the following steps:
and when the speed dv of the newly detected obstacle is greater than 0, determining the obstacle as a dynamic obstacle, and estimating the distance between the unmanned plane position odomPt before the time t according to the current position dp and the current speed dv of the dynamic obstacle to represent the collision probability.
When the velocity dv of the newly detected obstacle approaches 0, the occupation probability of the local occupation grid map fusion _ localMap at the expected track point prep at the time t is acquired by the getOccupy function.
Step 6: and (4) performing online re-planning. And determining the re-planning time, updating the expected track point of the unmanned aerial vehicle which leads the current time by a period of time t _ duration from the starting point start _ pt of the collaborative obstacle avoidance planning, and re-executing the steps 3, 4 and 5. The timing of the replanning includes the following:
(1) the euclidean distance of the local target point to the location of the drone is close to a certain threshold and the drone is not close to the end point.
(2) The value of the collision index obtained in step 5 is equal to 1.
(3) The distance between the unmanned aerial vehicle and the neighboring unmanned aerial vehicle is greater than the cooperative distance or less than the safe distance.
It should be noted that the detailed description is only for explaining and explaining the technical solution of the present invention, and the scope of protection of the claims is not limited thereby. It is intended that all such modifications and variations that fall within the spirit and scope of the invention be limited only by the claims and the description.
Claims (10)
1. A planning method for multi-unmanned aerial vehicle collaborative obstacle avoidance is characterized by comprising the following steps:
the method comprises the following steps: setting task scene parameters and task parameters;
the task scene parameters include: unmanned aerial vehicle number N with same configuration and size of task environment scene uav And a grid dispersion resolution;
the task parameters comprise: starting point coordinates start _ pt and end point coordinates end _ pt of the collaborative obstacle avoidance planning and the detectable distance of the unmanned aerial vehicle, wherein the detectable distance of the unmanned aerial vehicle is sense _ horizon;
step two: the unmanned aerial vehicle carries out isomorphic information exchange with an adjacent unmanned aerial vehicle through an unmanned aerial vehicle communication neighbor table;
the isomorphic information comprises perception information and flight state information;
the perception information comprises coordinates of the current position of the unmanned aerial vehicle in a world coordinate system and a local occupation grid map held by the unmanned aerial vehicle;
the flight state information comprises the global path and the expected track of the unmanned aerial vehicle;
the local occupancy grid map construction steps are as follows:
firstly, discretizing the size of a task environment scene by using grid dispersion degree resolution to obtain the size of a local occupied grid map, then continuously acquiring the position and the geometric property of an obstacle in a self-sensing range by using an unmanned aerial vehicle through an airborne sensor in the flight process, performing convex expansion processing on the position and the geometric property of the obstacle to obtain an obstacle grid area, and finally constructing the local occupied grid map according to the size of the local occupied grid map and the obstacle grid area;
step three: designing a distributed information fusion algorithm based on a maximum consistency theory and a distributed communication strategy, then carrying out iterative updating on a local occupancy grid map held by a current unmanned aerial vehicle by using the distributed information fusion algorithm and perception information exchanged by the current unmanned aerial vehicle and other unmanned aerial vehicles, setting an identification cache pool, stopping the iteration when the information in the identification cache pool is not updated, and at the moment, obtaining the local occupancy grid map held by the unmanned aerial vehicle as an optimal global occupancy grid map G;
step four: searching a global path with shortest barrier-free time between a start coordinate start _ pt and an end coordinate end _ pt of the collaborative obstacle avoidance planning;
step five: selecting a local target point tag _ pt in a sense _ horizon range by using an unobstructed time shortest global path, and performing track optimization on a path between a starting point coordinate start _ pt and the local target point tag _ pt of the collaborative obstacle avoidance plan to obtain a smooth feasible expected track leading from the starting point coordinate start _ pt to the local target point tag _ pt;
step six: and D, performing collision detection on the smooth feasible expected track obtained in the step five by using the optimal global occupancy grid map G obtained in the step three, wherein the collision detection specifically comprises the following steps:
setting forward check time t _ head and emergency braking time t _ stop, and then calculating collision probability p _ collision, wherein the collision probability p _ collision is expressed as:
dv is the speed of a newly detected obstacle, dp is the current position of the obstacle, odomPt is the current position of the unmanned aerial vehicle, getOccupy is an occupation probability acquisition function of an optimal global occupation grid map G, and prePt is an expected track point of the unmanned aerial vehicle at the moment t;
if the collision probability p _ collision is larger than 0.5, judging that the smooth feasible expected track and the newly detected obstacle generate collision conflict;
setting a collision index as 1 when the collision and collision opportunity is within the forward check time t _ head; when the collision conflict opportunity is within the braking time t _ stop, the unmanned aerial vehicle enters an emergency hovering state;
step seven: when the re-planning time is reached, updating the starting point start _ pt of the collaborative obstacle avoidance planning to be an expected track point advanced by 0.5 second at the current moment of the unmanned aerial vehicle, and re-executing the fourth step, the fifth step and the sixth step;
the replanning time is as follows:
(1) the Euclidean distance between the tag _ pt of the local target point and the current position odomPt of the unmanned aerial vehicle reaches a set threshold, and the unmanned aerial vehicle does not reach the terminal point;
(2) the collision index value obtained in the fifth step is equal to 1;
(3) the distance between the current unmanned aerial vehicle and the neighbor unmanned aerial vehicle is larger than the cooperative distance or smaller than the safe distance, and the distance between the current unmanned aerial vehicle and the neighbor unmanned aerial vehicle is obtained through flight state information exchanged between the current unmanned aerial vehicle and other unmanned aerial vehicles.
2. The planning method for collaborative obstacle avoidance by multiple unmanned aerial vehicles according to claim 1, wherein the barrier-free time shortest global path in step four is obtained by the following steps:
the method comprises the steps of simulating wave propagation in an environment by utilizing a local occupied grid map of a current unmanned aerial vehicle and taking a start coordinate start _ pt as a wave source, evaluating adjacent nodes of current propagation environment nodes in the propagation process to obtain evaluation values, selecting the adjacent node with the minimum evaluation value as a next propagation environment node, finishing the propagation process when the wave is expanded to an end coordinate end _ pt, tracing back the propagation path at the end coordinate end _ pt along the gradient descending direction of arrival time to obtain the propagation path with the shortest time from the end coordinate end _ pt to the start coordinate start _ pt, namely the barrier-free shortest time global path.
3. The planning method for collaborative obstacle avoidance of multiple unmanned aerial vehicles according to claim 2, wherein the evaluation value is obtained through an evaluation function f (n) of a current propagation environment node, the evaluation function f (n) includes a time cost function t (n), a heuristic information function h (n), and a collaborative obstacle avoidance profit function e (n);
the evaluation function f (n) is expressed as:
f(n)＝γ t T(n)+γ h H(n)-γ e E(n)
wherein, γ t ,γ h ,γ e The time cost function T (n), the heuristic information function H (n) and the cooperative obstacle avoidance profit function E (n) are respectively the proportionality coefficients in the evaluation function f (n).
4. A planning method for collaborative obstacle avoidance by multiple drones according to claim 3, wherein the time cost function t (n) is expressed as:
wherein, T x ,T y ,T z The minimum of the arrival times of all neighbors of grid n in x, y, z directions, respectively, and T x ＞T y ＞T z H is the spacing of adjacent grids and F is the propagation velocity of the wave at grid n.
The F is obtained through a speed field function, and the speed field function is established according to a local occupation grid map and by utilizing an ESDF map;
the velocity field function is expressed as:
the heuristic information function H (n) is expressed as:
the collaborative obstacle avoidance revenue function e (n) is expressed as:
wherein v is max D is the signed Euclidean distance of the current grid n from the nearest obstacle obtained from the ESDF map, e is the Euler constant of about 2.71828,
is a hyperbolic tangent function; dx, dy and dz are respectively the distance difference between the grid n and the end point coordinate end _ pt in the x, y and z directions, a ij For the communication relationship between drone i and drone j, d ij The deviation of the reference virtual center corresponds to drone i and drone j.
5. The planning method for collaborative obstacle avoidance of multiple unmanned aerial vehicles according to claim 1, characterized in that the concrete steps of the fifth step are:
step five, first: establishing an expected track model of the unmanned aerial vehicle based on the piecewise continuous eight-order Bezier curve;
step five two: taking a set of control points in an expected trajectory model of the unmanned aerial vehicle as an optimized object, solving a minimization problem, namely a trajectory optimization problem, of a flight energy consumption cost Je and a coordinated structure consistent cost Jc in a barrier-free safety convex set, solving according to the trajectory optimization problem to obtain an optimized object value, and finally bringing the optimized object value into the expected trajectory model to obtain a smooth feasible expected trajectory leading from a start coordinate start _ pt to a local target point tag _ pt.
6. A planning method for collaborative obstacle avoidance by multiple drones according to claim 5, wherein the expected trajectory model of the drones is expressed as:
wherein m is the number of track segments in the expected track model, n is the order of a Bezier curve in the expected track model, i is a mark and i belongs to [0, n]
Is an n-order Bezier curve basis function, i.e. Bernstein polynomial, p 1,i 、p 2,i …p m,i Segmenting track sections in an expected track model, wherein the track sections are n-order Bezier curve control points corresponding to the tracks with the labels of 1, 2. T is a time parameter, T 0 、T m Set respective start and end times, T, for the expected trajectory 0 、T 1 …T m-1 Respectively, a track start time, s, which is denoted by track segment reference number 1,2 1 、s 2 …s m Scaling a trajectory time interval scale factor, labeled 1, 2.. eta.m, for a trajectory segment in the expected trajectory model, for scaling a time parameter T from a time interval [ T [ ] m-1 ,T m ]Scaling to fixed time interval [0,1 ]]。
7. The planning method for collaborative obstacle avoidance by multiple drones according to claim 6, wherein the optimization object is represented as:
wherein p is 1,0 …p 1,8 …p m,0 …p m,8 Segmenting the track in the expected track model by using the n-order Bezier curve control point p corresponding to the track with the label of 1,2 1,i 、p 2,i …p m,i Taking n as 8, i belongs to [0, n ]]The detailed description of (1); x is the number of 1,0 …x 1,8 …x m,0 …x m,8 、y 1,0 …y 1,8 …y m,0 …y m,8 、z 1,0 …z 1,8 …z m,0 …z m,8 Respectively, the track segments in the expected track model are labeled as 1,2, 1,i 、p 2,i …p m,i taking n as 8, i belongs to [0, n ]]Expanded representation in three dimensions x, y, z.
8. The planning method for collaborative obstacle avoidance of multiple drones according to claim 7, wherein the trajectory optimization problem is expressed as:
min J＝λ e J e +λ c J c
wherein J is an objective function of the trajectory optimization problem, J e As a flight energy consumption cost function, J c For co-structural uniform cost functions, λ e 、λ c Is in proportionThe coefficient, N is the number of neighboring drones of the current drone i, j is the number of neighboring drones, aij represents the communication relationship between drone i and drone j,
corresponding to the reference virtual center positions, f, for UAV i and UAV j, respectively i (t) is the position of the drone i at time t, O i Is the reference virtual center vector for drone i.
9. The planning method for collaborative obstacle avoidance by multiple unmanned aerial vehicles according to claim 5, wherein the specific step of obtaining a smooth feasible expected trajectory leading from a start coordinate start _ pt to a local target point tag _ pt comprises:
step 1: intercepting a path between a start point coordinate start _ pt and a local target point tag _ pt from the barrier-free time shortest global path obtained in the fourth step, expanding each grid in the path into a largest cube in a safe space according to the grid, and finishing expansion when the boundary of the cube touches a barrier or the expansion times reach safe _ ite;
taking the number of the cubes after the largest cubes in the safety space are simplified as the number m of the sections of the track in the expected track model;
setting a track segment starting point state for the 1 st track in the segment number m of the track, wherein the starting point state comprises a position start _ pt, a speed start _ vel and an acceleration start _ acc at the starting point of the unmanned aerial vehicle, and is represented as follows:
p 1,0 ·s 1 ＝start_pt
8(p 1,1 -p 1,0 )＝start_vel
setting a track segment end point state for the mth track in the segment number m of the track, wherein the end point state corresponds to the local target point position tag _ pt, and the speed and the acceleration are set to be zero, and are expressed as follows:
p m,5 ·s m ＝tag_pt
8(p m,6 -p m,5 )＝[0,0,0]
step 2: adding a maximum speed limit, an acceleration limit and a continuity limit between the segmented tracks to the expected track model;
the maximum speed limit is:
-v max ≤n·(p j,i -p j,i-1 )≤v max ,i＝1,2,...,n
the acceleration limit is expressed as:
the inter-segment trajectory continuity constraint is expressed as:
s j ·p j,n -s j+1 ·p j+1,0 ＝0
(p j,n -p j,n-1 )-(p j+1,1 -p j+1,0 )＝0
and step 3: taking the simplified cube as a domain, integrating the contents in the step 1 and the step 2 to obtain a quadratic programming problem, wherein the solution of the quadratic programming problem is the position of a control point in an expected trajectory model of the unmanned aerial vehicle, and further obtaining a smooth feasible expected trajectory leading to a local target point;
wherein n is the order of the Bezier curve in the expected track model, v max 、a max Maximum velocity and acceleration, p, respectively j,i-2 、p j,i-1 、p j,i 、p j,n-2 、p j,n-1 、p j,n Control points p with the labels i-2, i-1, i, n-2 and n in the n-order Bezier curve corresponding to the jth track in the number m of the track sections j+1,0 、p j+1,1 、p j+1,2 Control points with labels of 0,1 and 2 in an nth-order Bezier curve corresponding to the j +1 th track in the number m of the track sections respectively, and s j 、s j+1 The track time interval scaling factors for the j-th and j + 1-th segments, respectively, in the number of segments m of the track.
10. A planning method for collaborative obstacle avoidance by multiple drones according to claim 9, wherein the quadratic planning problem is expressed as:
min p T Qp+C T p+c f
s.t.p j ∈Ω j ,j＝1,2,...,m
A eq p＝b eq
l ieq ≤A ieq p≤u ieq
wherein, the number of segments of the m tracks, Q is the Hessian matrix of the objective function J, C is the linear coefficient matrix of the objective function J, C f A constant term of the objective function J, p is an n-order Bezier curve control point p corresponding to a track with a track segment label of 1,2 1,i 、p 2,i …p m,i Is represented by a matrix of p j Is a matrix representation of the control points of the Bezier curve of the n-th order corresponding to the jth track in the number m of the sections of the track, omega j Is a matrix p j Of barrier-free safety convex sets, i.e. matrices p j Definition domain of A eq 、b eq Coefficient matrix and constant matrix constrained by equation of objective function J, A ieq Coefficient matrix constrained by inequality of objective function J,/ ieq 、u ieq A corresponding minimum constant matrix and maximum constant matrix.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210608189.7A CN114967744A (en) | 2022-05-31 | 2022-05-31 | Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210608189.7A CN114967744A (en) | 2022-05-31 | 2022-05-31 | Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114967744A true CN114967744A (en) | 2022-08-30 |
Family
ID=82958373
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210608189.7A Pending CN114967744A (en) | 2022-05-31 | 2022-05-31 | Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114967744A (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115373426A (en) * | 2022-10-26 | 2022-11-22 | 四川腾盾科技有限公司 | Area coverage online path collaborative planning method for fixed wing cluster unmanned aerial vehicle |
| CN115617078A (en) * | 2022-12-12 | 2023-01-17 | 北京理工大学 | Unmanned aerial vehicle three-dimensional flight path rapid planning method based on puffing obstacle |
| CN116257082A (en) * | 2022-11-03 | 2023-06-13 | 天津大学 | Distributed active cooperative detection method for multiple unmanned aerial vehicles |
| CN116893687A (en) * | 2023-08-21 | 2023-10-17 | 广东工业大学 | Unmanned aerial vehicle path planning method for improving lazy theta in complex environment |
-
2022
- 2022-05-31 CN CN202210608189.7A patent/CN114967744A/en active Pending
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115373426A (en) * | 2022-10-26 | 2022-11-22 | 四川腾盾科技有限公司 | Area coverage online path collaborative planning method for fixed wing cluster unmanned aerial vehicle |
| CN115373426B (en) * | 2022-10-26 | 2023-02-17 | 四川腾盾科技有限公司 | Area coverage online path collaborative planning method for fixed wing cluster unmanned aerial vehicle |
| CN116257082A (en) * | 2022-11-03 | 2023-06-13 | 天津大学 | Distributed active cooperative detection method for multiple unmanned aerial vehicles |
| CN116257082B (en) * | 2022-11-03 | 2023-09-22 | 天津大学 | Distributed active cooperative detection method for multiple unmanned aerial vehicles |
| CN115617078A (en) * | 2022-12-12 | 2023-01-17 | 北京理工大学 | Unmanned aerial vehicle three-dimensional flight path rapid planning method based on puffing obstacle |
| CN116893687A (en) * | 2023-08-21 | 2023-10-17 | 广东工业大学 | Unmanned aerial vehicle path planning method for improving lazy theta in complex environment |
| CN116893687B (en) * | 2023-08-21 | 2024-01-23 | 广东工业大学 | Unmanned aerial vehicle path planning method for improving lazy theta in complex environment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114967744A (en) | Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance | |
| CN111240319B (en) | Outdoor multi-robot cooperative operation system and method thereof | |
| CN110428111B (en) | UAV/UGV (unmanned aerial vehicle/user generated Union vector) cooperative long-time multitask operation trajectory planning method | |
| CN109947136B (en) | Collaborative active sensing method for unmanned aerial vehicle group rapid target search | |
| Lindhé et al. | Flocking with obstacle avoidance: A new distributed coordination algorithm based on voronoi partitions | |
| CN109508035A (en) | Multizone stagewise unmanned plane formation paths planning method based on distributed AC servo system | |
| CN114355981B (en) | Method and system for autonomous exploration and mapping of four-rotor unmanned aerial vehicle | |
| CN107972034B (en) | Complex workpiece trajectory planning simulation system based on ROS platform | |
| CN110531770A (en) | One kind being based on improved RRT paths planning method and system | |
| CN110471426A (en) | Unmanned intelligent vehicle automatic Collision Avoidance method based on quantum wolf pack algorithm | |
| CN114740846A (en) | Hierarchical path planning method for topology-grid-metric hybrid map | |
| CN113253744B (en) | Multi-robot collaborative trajectory planning method and device, electronic equipment and storage medium | |
| Gao et al. | Multi-mobile robot autonomous navigation system for intelligent logistics | |
| CN105204511B (en) | A kind of decision-making technique of object autonomous | |
| CN112083727A (en) | Multi-autonomous system distributed collision avoidance formation control method based on speed barrier | |
| CN115454115A (en) | Rotor unmanned aerial vehicle path planning method based on hybrid wolf-particle swarm algorithm | |
| CN112859110A (en) | Positioning and navigation method based on three-dimensional laser radar | |
| CN115562357A (en) | Intelligent path planning method for unmanned aerial vehicle cluster | |
| Zhang et al. | Research on obstacle avoidance algorithm of multi-UAV consistent formation based on improved dynamic window approach | |
| Ali et al. | Feature selection-based decision model for UAV path planning on rough terrains | |
| Belavadi et al. | Frontier exploration technique for 3d autonomous slam using k-means based divisive clustering | |
| Ma et al. | Volcanic Ash Region Path Planning Based on Improved A‐Star Algorithm | |
| CN111176324B (en) | Method for avoiding dynamic obstacle by multi-unmanned aerial vehicle distributed collaborative formation | |
| Niendorf et al. | Multi-query path planning for an unmanned fixed-wing aircraft | |
| Han et al. | Research on UAV indoor path planning algorithm based on global subdivision grids |
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 |