CN118192608A - Distributed online multi-machine track planning method and system - Google Patents
Distributed online multi-machine track planning method and system Download PDFInfo
- Publication number
- CN118192608A CN118192608A CN202410492778.2A CN202410492778A CN118192608A CN 118192608 A CN118192608 A CN 118192608A CN 202410492778 A CN202410492778 A CN 202410492778A CN 118192608 A CN118192608 A CN 118192608A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- track
- planning
- aerial vehicle
- constraint
- 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 37
- 230000003068 static effect Effects 0.000 claims abstract description 18
- 230000001360 synchronised effect Effects 0.000 claims abstract description 15
- 230000001133 acceleration Effects 0.000 claims description 11
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 description 24
- 238000005457 optimization Methods 0.000 description 12
- 238000004422 calculation algorithm Methods 0.000 description 8
- 238000010276 construction Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 5
- 238000004891 communication Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 238000005096 rolling process Methods 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 238000010845 search algorithm Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 230000002265 prevention Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000000926 separation method Methods 0.000 description 2
- GZDFHIJNHHMENY-UHFFFAOYSA-N Dimethyl dicarbonate Chemical compound COC(=O)OC(=O)OC GZDFHIJNHHMENY-UHFFFAOYSA-N 0.000 description 1
- 241000283283 Orcinus orca Species 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 239000003795 chemical substances by application Substances 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000007723 transport mechanism Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a distributed online multi-machine track planning method and a system; by constructing a re-planning track of the unmanned aerial vehicle; constructing a safety flight corridor of the unmanned aerial vehicle according to the estimated boundary and the coordinates of the static obstacle so as to obtain a first constraint of the unmanned aerial vehicle for avoiding the static obstacle; simplifying collision constraint among unmanned aerial vehicles into second constraint of obstacle avoidance among the unmanned aerial vehicles with convex constraint according to the prior track; obtaining a third constraint of the unmanned aerial vehicle for avoiding the obstacle dynamic obstacle according to the radius of the unmanned aerial vehicle, the radius of the dynamic obstacle and the track prediction error of the dynamic obstacle as required; grouping unmanned aerial vehicles, and carrying out track planning on each group of unmanned aerial vehicles according to an objective function, the first constraint, the second constraint and the third constraint of a re-planning track in a mode of inter-group asynchronous planning and intra-group synchronous planning to obtain a target track; the solving speed of track planning is improved, time-sharing solving is realized, the load is uniformly spread, and the instantaneous resources are saved.
Description
Technical Field
The invention belongs to the field of unmanned aerial vehicle control, and particularly relates to a distributed online multi-machine track planning method and system.
Background
Unmanned aerial vehicle is a branch in the robot field, has extensive application scenario. Due to the limitation of conditions such as volume, load and energy, a single unmanned aerial vehicle is difficult to meet the requirements of complex tasks, and the cooperation of multiple unmanned aerial vehicles is the current main application direction.
Given a set of unmanned aerial vehicles with known initial positions and target positions, a set of continuous functions is found, each unmanned aerial vehicle is moved from the initial position to the target position, and the unmanned aerial vehicle track planning problem is solved. Because a large number of unmanned aerial vehicles share the same space, the planning algorithm must consider the environment and the states of adjacent unmanned aerial vehicles, and the unmanned aerial vehicles are prevented from collision.
Regarding continuous multi-unmanned aerial vehicle trajectory optimization, the earliest developed techniques are Mixed-integer linear Programming (MILP) and Mixed-integer quadratic Programming (MIQP) (MixedInteger Quadratic Programming, MIQP), but these methods jointly optimize the trajectories of all unmanned aerial vehicles, while guaranteeing optimality, but can only be applied to scenes where both obstacles and the number of unmanned aerial vehicles are small due to time complexity. The collision constraint among unmanned aerial vehicles is decoupled by utilizing sequential planning, so that the calculation efficiency is remarkably improved, but the situation of feasible solution loss exists. The methods based on the distributed and reactive methods are buffered Voronoi cells (Buffer Voronoi Cells, BVC), velocity barrier (Velocity Obstacle, VO), optimal reciprocal collision avoidance (Optimal Reciprocal CollisionAvoidance, ORCA), etc., but none of these methods optimize the target trajectory. The multi-agent offline track generation algorithm based on distributed model predictive control (Distributed Model Predictive Control, DMPC) generally adopts a track description based on discrete time points, and has the problems that the optimization variable is huge, the solving speed is low, and the safety between the discrete points cannot be ensured.
Meeting and compatible diverse applications requires more generalizing the definition of the planning problem, dropping potential optimizations for specific problems that can improve optimality and reduce computation time. Increasing the optimality of the trajectory requires more complex and fine modeling of the problem, and more iterations in the feasible space, which necessarily comes at the cost of increased planning time. These problems all lead to an increase in the trajectory solving time of the unmanned aerial vehicle and a decrease in the trajectory solving efficiency.
Disclosure of Invention
The invention aims to at least solve one of the technical problems in the prior art, and provides a distributed online multi-machine track planning method and system, so that track solving efficiency is improved.
An embodiment of the first aspect of the present invention is a distributed online multi-machine trajectory planning method, including the steps of:
constructing a re-planning track of the unmanned aerial vehicle, wherein the re-planning track comprises track segments of all time periods, and the track segments comprise at least one control point;
obtaining the maximum flight distance of the unmanned aerial vehicle according to the maximum speed of the unmanned aerial vehicle, obtaining a predicted boundary according to the maximum flight distance, constructing a safety flight corridor of the unmanned aerial vehicle according to the predicted boundary and the coordinates of the static obstacle, and obtaining a first constraint according to the safety flight corridor;
predicting a priori trajectory according to the speed of the unmanned aerial vehicle, establishing collision constraint between the unmanned aerial vehicles according to the radius of the unmanned aerial vehicle, and simplifying the collision constraint between the unmanned aerial vehicles into a second constraint of convex constraint according to the priori trajectory;
Obtaining a maximum flight spherical region of the unmanned aerial vehicle according to the maximum speed of the unmanned aerial vehicle, obtaining a predicted track of the dynamic obstacle according to the speed of the dynamic obstacle, obtaining a track prediction error of the dynamic obstacle according to the maximum acceleration of the dynamic obstacle, and obtaining a third constraint according to the radius of the unmanned aerial vehicle, the radius of the dynamic obstacle and the track prediction error of the dynamic obstacle when the predicted track of the dynamic obstacle is in the maximum flight spherical region of the unmanned aerial vehicle;
Grouping unmanned aerial vehicles, and carrying out track planning on each group of unmanned aerial vehicles according to the first constraint, the second constraint and the third constraint in a mode of inter-group asynchronous planning and intra-group synchronous planning to obtain a target track.
According to certain embodiments of the first aspect of the present invention, the re-planned trajectory of the ith drone is represented as: wherein p i (t) is a rescheduled track; b l,n is Bernstein-based polynomial; n is the order of the polynomial; /(I) Τ m is the normalized value of time t; /(I)An ith control point segmented for an ith track of the ith drone.
According to certain embodiments of the first aspect of the present invention, the constructing a safe flight corridor of the unmanned aerial vehicle according to the coordinates of the estimated boundary and the static obstacle includes:
Performing expansion operation from the starting point coordinates to a preset direction until an expansion area reaches an estimated boundary, the coordinates of a static obstacle or a map boundary, and obtaining a safety flight corridor planned for the first time;
the safe flight corridor for the a-th plan is expressed as: Wherein M is the number of segments of the track segment, k is the number of steps of the reprofiling,/> Safety flight corridor for the ith unmanned aerial vehicle at the mth segment of the kth step re-planning,/>For the ith unmanned plane, in the (m+1) -th section of the safety flight corridor re-planned in the (k-1) -th step, S is the expansion operation until the expansion area reaches the estimated boundary, the coordinates of the static obstacle or the map boundary, and S i is the initial coordinate,/>Is the predicted control point.
According to certain embodiments of the first aspect of the present invention, the a priori trajectory is represented as: Wherein/> For a priori trajectory, M is the number of segments of the trajectory segment, k is the number of steps for the reprofiling, s i is the starting coordinate,/>Is the speed of the unmanned aerial vehicle, t is time,/>Predictive control point for the ith section of the ith drone re-planned at the kth step,/>Is the predicted speed.
According to certain embodiments of the first aspect of the present invention, the second constraint is expressed as: Wherein/> P i (t) is the re-planned trajectory of the ith unmanned aerial vehicle,/>For the i-th unmanned aerial vehicle's a priori trajectory,/>For the prior trajectory of the jth drone, r i is the radius of the ith drone, and r j is the radius of the jth drone.
According to certain embodiments of the first aspect of the present invention, the third constraint is expressed as: Wherein, p i (t) is the re-planning track of the ith unmanned aerial vehicle,/> R i is the radius of the ith unmanned aerial vehicle, r obs is the radius of the dynamic obstacle, and/(I)Is the trajectory prediction error of the dynamic obstacle.
According to certain embodiments of the first aspect of the present invention, the objective function of the re-planning trajectory is: j=j d+Je, J is an objective function, J d is an energy objective function, and J e is an error objective function; w d is the weight parameter of the energy objective function, w e is the error objective function, T 0 is the start time, T M is the end time, phi is the derivative order, M is the number of segments of the track segment, k is the number of steps of the programming,And (3) the control point is the first control point of the mth track segment of the ith unmanned aerial vehicle in the kth step re-planning.
According to certain embodiments of the first aspect of the present invention, the inter-group asynchronous planning and intra-group synchronous planning are in the following manner:
a plurality of unmanned aerial vehicles in the same group synchronously carry out track planning in the same time period;
The ith group of unmanned aerial vehicles performs track planning in the ith time period, and the interval between two adjacent time periods is equal to or several times of the time of the track segmentation.
An embodiment of the second aspect of the application is a computer readable storage medium storing program instructions which, when executed by a processor, implement a distributed online multi-machine trajectory planning as described above.
An embodiment of the third aspect of the present application is a distributed online multi-machine trajectory planning system, comprising: a computer apparatus comprising a computer readable storage medium according to the above.
The beneficial effects of the invention include: by constructing a re-planning track of the unmanned aerial vehicle; constructing a safety flight corridor of the unmanned aerial vehicle according to the estimated boundary and the coordinates of the static obstacle so as to obtain a first constraint of the unmanned aerial vehicle for avoiding the static obstacle; simplifying collision constraint among unmanned aerial vehicles into second constraint of obstacle avoidance among the unmanned aerial vehicles with convex constraint according to the prior track; obtaining a third constraint of the unmanned aerial vehicle for avoiding the obstacle dynamic obstacle according to the radius of the unmanned aerial vehicle, the radius of the dynamic obstacle and the track prediction error of the dynamic obstacle as required; grouping unmanned aerial vehicles, and carrying out track planning on each group of unmanned aerial vehicles according to an objective function, the first constraint, the second constraint and the third constraint of a re-planning track in a mode of inter-group asynchronous planning and intra-group synchronous planning to obtain a target track; the solving speed of track planning is improved, time-sharing solving is realized, the load is uniformly spread, and the instantaneous resources are saved.
Further, additional aspects and advantages of the invention will be set forth in part in the description which follows and, in part, will be obvious from the description, or may be learned by practice of the invention.
Drawings
FIG. 1 is a step diagram of a distributed online multi-machine trajectory planning method;
FIG. 2 is a flow chart of an improved safe flight corridor construction algorithm;
FIG. 3 is a schematic illustration of obstacle avoidance constraints of a drone and a static obstacle;
FIG. 4 is a geometric sense of obstacle avoidance constraints between unmanned aerial vehicles;
FIG. 5 is a schematic illustration of obstacle avoidance constraints between unmanned aerial vehicles;
FIG. 6 is a schematic diagram of time stamps for an inter-group asynchronous plan and an intra-group synchronous plan;
FIG. 7 is a diagram of simulation results of a distributed online multi-machine trajectory planning method;
Fig. 8 is a block diagram of the unmanned system and a ground computer.
Detailed Description
The conception, specific structure, and technical effects produced by the present application will be clearly and completely described below with reference to the embodiments and the drawings to fully understand the objects, aspects, and effects of the present application. It should be noted that, without conflict, the embodiments of the present application and features of the embodiments may be combined with each other.
It should be noted that, unless otherwise specified, when a feature is referred to as being "fixed" or "connected" to another feature, it may be directly or indirectly fixed or connected to the other feature. Further, the descriptions of the upper, lower, left, right, top, bottom, etc. used in the present invention are merely with respect to the mutual positional relationship of the respective constituent elements of the present invention in the drawings.
Furthermore, unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art. The terminology used in the description presented herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "and/or" as used herein includes any combination of one or more of the associated listed items.
For autonomous unmanned aerial vehicles, the track planning is often used as a downstream of real-time perception, and real-time online re-planning is required to be performed by combining with real-time updated perception information. The online re-planning puts forward high demands on planning time, the optimization problem of directly establishing obstacle constraint and unmanned aerial vehicle constraint is nonlinear and non-convex, and the situations of low solving speed, difficult convergence or convergence to a local minimum value easily occur, so that the flight effect is poor.
The embodiment of the invention provides a distributed online multi-machine track planning method which is applied to an unmanned aerial vehicle system.
Referring to fig. 1, the distributed online multi-machine track planning method includes the following steps:
step S100, a re-planning track of the unmanned aerial vehicle is constructed;
Step S200, obtaining the maximum flight distance of the unmanned aerial vehicle according to the maximum speed of the unmanned aerial vehicle, obtaining a predicted boundary according to the maximum flight distance, constructing a safe flight corridor of the unmanned aerial vehicle according to the predicted boundary and the coordinates of the static obstacle, and obtaining a first constraint according to the safe flight corridor;
Step S300, predicting a priori trajectory according to the speed of the unmanned aerial vehicle, establishing collision constraint between the unmanned aerial vehicles according to the radius of the unmanned aerial vehicle, and simplifying the collision constraint between the unmanned aerial vehicles into a second constraint of convex constraint according to the priori trajectory;
Step S400, obtaining a maximum flight spherical region of the unmanned aerial vehicle according to the maximum speed of the unmanned aerial vehicle, obtaining a predicted track of the dynamic obstacle according to the speed of the dynamic obstacle, obtaining a track prediction error of the dynamic obstacle according to the maximum acceleration of the dynamic obstacle, and obtaining a third constraint according to the radius of the unmanned aerial vehicle, the radius of the dynamic obstacle and the track prediction error of the dynamic obstacle when the predicted track of the dynamic obstacle is in the maximum flight spherical region of the unmanned aerial vehicle;
And S500, grouping unmanned aerial vehicles, and carrying out track planning on each group of unmanned aerial vehicles according to an objective function, a first constraint, a second constraint and a third constraint of a re-planned track in a mode of inter-group asynchronous planning and intra-group synchronous planning to obtain a target track.
For step S100, a re-planning track of the unmanned aerial vehicle is constructed; the re-planned trajectory comprises trajectory segments for respective time periods, the trajectory segments comprising at least one control point.
The re-planned trajectory of the ith unmanned aerial vehicle in the kth re-planning step is expressed as: wherein p i (t) is a rescheduled track; b l,n is Bernstein-based polynomial; n is the order of the polynomial; /(I) Τ m is the normalized value of time t; /(I)An ith control point segmented for an ith track of the ith drone.
For the objective function of re-planning the trajectory, minimizing energy, i.e. trajectory, to ensure that the trajectory is smooth and continuousThe integral of the second-norm square of the derivative over the reprofiling time domain is taken as the energy cost and given a weight w d, the energy objective function J d is expressed as: /(I)Wherein J d is the energy objective function, w d is the weight parameter of the energy objective function, T 0 is the start time, T M is the end time, φ is the derivative order,/>And (3) the control point is the first control point of the mth track segment of the ith unmanned aerial vehicle in the kth step re-planning.
Because the online re-planning belongs to the rolling optimization problem in a certain time domain, the target point is not applied as a hard constraint, in order to enable the track to reach the target point quickly, the square sum of the position of the future kappa step of the unmanned plane to the end point distance is minimized and is used as an error cost, the weight w e is given, and the error objective function J e is expressed as: Wherein J e is the error objective function, w e is the error objective function, M is the number of segments of the track segment, and k is the number of steps of the reprofiling.
Both objective function terms can be expressed as quadratic forms of the control points. The energy cost and the error cost are respectively provided with weight matrixes, and the energy cost and the error cost are balanced by adjusting proper weight coefficients, so that the vibration problem is avoided.
The total cost J of the objective function is: j=j d+Je.
For step S200, by means of the convex hull property of the control points of the Bernstein-based polynomial, the control points are constrained in the convex flight safety corridor SFC, so that the unmanned aerial vehicle can be ensured not to collide with irregular obstacles in the environment.
The axis searching algorithm sequentially expands towards the six directions of the positive x-axis direction, the negative x-axis direction, the positive y-axis direction, the negative y-axis direction, the positive z-axis direction and the negative z-axis direction at the point to be expanded until all directions touch the coordinates of the static obstacle or the map boundary. However, the axis search algorithm expansion process is time consuming, and in this embodiment, the improvement of the SFC build by virtue of the nature of the online re-planning does not require searching all feasible convex spaces in the environment. The dynamic limit of the unmanned plane is utilized to improve the axial search algorithm of the SFC, and expansion of each axis of the SFC to a dynamic limit area or an obstacle area or a boundary area can stop expansion in the direction.
At the time of initial planning, the starting point coordinates s i are used as the initial SFC. Obtaining the maximum flight distance of the unmanned aerial vehicle according to the maximum speed of the unmanned aerial vehicle, wherein the maximum flight distance is deltap=v max (1+a) deltat; wherein a is an adjustment parameter. And obtaining a predicted boundary according to the maximum flight distance. And performing expansion operation from the starting point coordinates to a preset direction according to an improved safe flight corridor construction algorithm until an expansion area reaches an estimated boundary, the coordinates of a static obstacle or a map boundary, so as to obtain the safe flight corridor planned for the first time.
Referring to fig. 2, the improved safe flight corridor construction algorithm may be implemented as follows: initializing the segment number m=1, the direction set d= { +x, -x, +y, -y, +z, -z }, the total number of directions e=6, and the direction index e=0. Judging whether M < M is satisfied, if so, initializing the mth segment SFC as an initial segmentAnd judging whether the set D is empty, if so, updating the segment number m=m+1, and if not, detecting whether the SFC encounters an obstacle in the direction of the index e. If the SFC does not meet the obstacle in the direction of the index e, judging whether the SFC extends to the estimated position boundary or not. If the SFC encounters an obstacle in the direction of index E or the SFC expands to the estimated position boundary, the corresponding direction of index E is removed from the set D, and e=e-1, e= (E-1+e)% E. If the SFC does not expand to the estimated position boundary, the SFC expands a grid in the direction of index e. Update direction index e= (e+1)% E. And updating the segment number m=m+1, returning to the step of judging whether M < M is satisfied, and ending the flow when M is greater than or equal to M.
For non-primary planning, the planning result of the last step can be used as the prediction information of the current planning, and the re-planning period is set to be equal to or multiple of the interval time of two sections of the piecewise polynomial. For the same case, the SFC of the 1 st to M th sections constructed in the k-1 st step can be directly applied to the 0 th to M-1 th sections planned in the k-th step, and new SFC is constructed without the need of re-axis searching, so that the SFC is obtained by direct scrolling. And the SFC of the Mth track without rolling prediction is constructed by an improved shaft search algorithm, so that the SFC is constructed in a rolling way, and the time complexity of the SFC construction is greatly reduced.
Specifically, the safe flight corridor for the a-th plan is expressed as: Wherein M is the number of segments of the track segment, k is the number of steps of the reprofiling,/> Safety flight corridor for the ith unmanned aerial vehicle at the mth segment of the kth step re-planning,/>For the ith unmanned plane, in the (m+1) -th section of the safety flight corridor re-planned in the (k-1) -th step, S is the expansion operation until the expansion area reaches the estimated boundary, the coordinates of the static obstacle or the map boundary, and S i is the initial coordinate,/>Is the predicted control point.
And obtaining first constraint of the unmanned aerial vehicle to obstacle avoidance of the static obstacle according to the safety flight corridor.
According to the method for directly constructing the constraint based on the rolling SFC, the obstacle avoidance constraint of the kth step can not be obtained directly and implicitly in a time period without searching the SFC again. The method for constructing SFC by improved axial search based on the re-planning step length and the dynamics limit reduces the space of search expansion and improves the efficiency. Thus, in distributed online multimachine planning, building SFC constraints does not suffer from the long computation time as in offline multimachine planning.
Referring to fig. 3, it is constituted by a plurality of convex rectangular parallelepiped constraints. The simulation experiment can obtain that in 8 simulation experiments without any planning, the average time of single planning in the original SFC constraint construction is 1.38ms, and the average time of single planning in the SFC constraint construction is improved to be 0.9ms. 2. The calculation time comparison results of the improved SFC which are not used in the planning process of 4, 8 and 16 unmanned aerial vehicles are as follows: the improved method time was 22.5% in the case of 2, 26% in the case of 4, 34.8% in the case of 8, and 35.2% in the case of 16.
For step S300, a priori trajectory is predicted according to the speed of the unmanned aerial vehicle, collision constraints between the unmanned aerial vehicles are established according to the radius of the unmanned aerial vehicle, and the collision constraints between the unmanned aerial vehicles are simplified to be second constraints of convex constraints according to the priori trajectory.
And for primary planning, predicting a priori trajectory according to the speed of the unmanned aerial vehicle.
The a priori trajectory is expressed as: Wherein/> For a priori trajectory, M is the number of segments of the trajectory segment, k is the number of steps for the reprofiling, s i is the starting coordinate,/>Is the speed of the unmanned aerial vehicle, t is the time,Predictive control point for the ith section of the ith drone re-planned at the kth step,/>Is the predicted speed.
For the k-th planning, the prior track of the 0 th to M-1 th sections can be directly obtained from the 1 st to M th sections of the k-1 th steps, and the planning result of the last step is adopted as the prior track of other unmanned aerial vehicles in the current planning; whereas the a priori trajectory of the M th segment followsAnd predicting to obtain a priori trajectory.
In order to describe obstacle avoidance constraint between unmanned aerial vehicles, firstly, a mutual collision model between unmanned aerial vehicles is established as follows: Wherein r i and r j are the radii of the ith and j unmanned aerial vehicle respectively, and e=diag ([ 1, 1/(c dw)2],cdw) is the coefficient of the downwash effect of the unmanned aerial vehicle, which is because the propeller wind force of the unmanned aerial vehicle will affect the Z axis direction of the fuselage during the flight, so that the collision distance of the Z axis direction of the fuselage is enlarged to be c dw times of the original distance.
The condition that collision does not occur between the ith unmanned aerial vehicle and the jth unmanned aerial vehicle is further deduced as follows:
Because the obstacle avoidance constraint between the unmanned aerial vehicles is a non-convex space, the optimization problem cannot be converged or the solving speed is low due to the fact that the non-convex constraint is directly applied. Therefore, the collision constraint between unmanned aerial vehicles is established according to the radius of the unmanned aerial vehicle by using the predicted trajectories of other unmanned aerial vehicles as priori information, namely, the ith unmanned aerial vehicle avoids the collision constraint of the jth unmanned aerial vehicle, and the collision constraint between the unmanned aerial vehicles is expressed as:
and simplifying collision constraint among unmanned aerial vehicles into a second constraint of convex constraint according to the prior track.
For a pair ofSimplified, let/>Taylor expansion at x 0 using f (x) =f (x 0)+f(1)(x0)(x-x0)+o(x-x0), preserving the first order linear term, discarding the higher order infinitely small, and simplifying the non-convex quadratic constraint into a convex linear constraint, yields:
Order the Simplifying to obtain a second constraint, wherein the second constraint is expressed as: /(I) Wherein/> P i (t) is the re-planned trajectory of the ith unmanned aerial vehicle,/>For the i-th unmanned aerial vehicle's a priori trajectory,/>For the prior trajectory of the jth drone, r i is the radius of the ith drone, and r j is the radius of the jth drone.
Referring to fig. 4, a normal vector and a corresponding tangent plane are obtained based on the minimum distance direction from the convex hull surrounded by the prediction track to the sphere center of the collision model, wherein the normal vector is the direction in which the nearest point to the sphere center of the sphere in the convex hull is located, and the tangent line is the tangent plane at the intersection point of the normal vector and the sphere. Geometrically, the normal vector and tangential plane may constitute an inter-machine collision constraint, with the non-convex feasible space simplified to a convex half-space. Regarding the solution of the closest point from the convex hull to the center of sphere, the GJK algorithm can be utilized to directly calculate the closest distance and direction vector of the two convex hulls.
Referring to fig. 5, it is composed of a plurality of linear half-space constraints. Searching for a separation hyperplane by utilizing a predicted priori path, and converting the separation hyperplane into a convex linear space; furthermore, collision prevention constraint among unmanned aerial vehicles is simplified, so that the collision prevention constraint is converted from the original non-convex quadratic equation to the current linear convex constraint, and the solving efficiency of the optimization problem is improved.
For step S400, constraints of the unmanned aerial vehicle on obstacle avoidance of the dynamic obstacle are constructed.
And establishing the maximum reachable range radius R of the unmanned aerial vehicle, and performing collision detection on the predicted track of the dynamic obstacle only in the maximum flying spherical area with the radius R, so as to reduce the collision constraint amount in optimization.
The maximum radius R is obtained from the maximum speed of the unmanned aerial vehicle according to the dynamics limit of the unmanned aerial vehicle and is as follows: r=v max(TM-T0). The predicted track of the dynamic obstacle is obtained according to the speed of the dynamic obstacle: The track prediction error of the dynamic obstacle is obtained according to the maximum acceleration of the dynamic obstacle, and is as follows: /(I) B is an adjustable threshold parameter.
Assuming the dynamic obstacle as a sphere, the collision model between the unmanned aerial vehicle and the dynamic obstacle can be obtained as follows:
When the predicted track of the dynamic obstacle is in the maximum flight spherical area of the unmanned aerial vehicle, obtaining obstacle avoidance constraint between the unmanned aerial vehicle and the dynamic obstacle according to the radius of the unmanned aerial vehicle, the radius of the dynamic obstacle and the track prediction error of the dynamic obstacle, namely third constraint; the third constraint is:
Due to The quadratic polynomial with respect to time t may be expressed as a Bernstein-based polynomial, so for the constraints of the drone and dynamic obstacle, the method of taylor expansion at the predicted trajectory may also be used to transform the non-convex constraint into a convex linear half-space constraint.
Regarding collision detection between the unmanned aerial vehicle and the dynamic obstacle, the unmanned aerial vehicle track and the dynamic obstacle track are Bernstein-based polynomials, so that the control point convex hull property can be utilized, the GJK algorithm is used for calculating the distance between two simplex, whether the distance is larger than the safety distance is judged, and the solving speed of collision detection is greatly improved.
In addition, the track needs to meet initial conditions and match the current position, speed and acceleration of the unmanned aerial vehicle, and then state constraint of an initial point needs to be added.
On-line re-planning does not need to add hard constraints of the end point state to the unmanned aerial vehicle, and the approach of the track towards the end point state exists as a cost in the objective function. The initial state constraints are:
for smooth flight, the trajectory should be high order continuous, and then the continuity constraint of the segmented Bernstein-based polynomial needs to be added.
The smoothness and continuity of the trajectory requires that the piecewise Bernstein-based polynomial satisfy the following continuity constraints, namely: from the nature of the derivatives of Bernstein-based polynomials, the reduction yields constraints on the control points as:
because the control quantity of the unmanned aerial vehicle has dynamics limitation, maximum speed and maximum acceleration constraint are required to be added, and the maximum speed and the maximum acceleration constraint are as follows: Since the derivative of the Bernstein-based polynomial is also a Bernstein-based polynomial, the velocity curve and the acceleration curve are also Bernstein-based polynomials, and the convex hull formed by the control points also has the property of surrounding the track, so that the control points of the velocity and acceleration curve can be directly constrained in the velocity and acceleration limiting, and the maximum velocity and maximum acceleration constraint is reduced to:
The state constraint and the continuity constraint about the starting point and the target point can be restated by a linear equation constraint a eqc＝beq, the constraint of the upper limit and the lower limit of the control quantity can be restated by a linear inequality constraint a ieqc≤bieq, and the constraints can be expressed as a linear equation or inequality form of a decision variable control point and are convex constraints of an optimization variable.
The online multi-machine track optimization problem of q n unmanned aerial vehicles is formed by an objective function formula, an unmanned aerial vehicle and obstacle avoidance constraint formula, an unmanned aerial vehicle inter-collision avoidance constraint formula, an initial state constraint formula, a continuity constraint formula and a dynamics constraint formula. Wherein J d and J e are the energy cost and the error cost of the objective function respectively,For the set of decision variable control points, a eq and b eq are matrix parameters for linear equality constraints in all constraints above, and a ieq and b ieq are matrix parameters for linear inequality constraints in all constraints above. The linear equality constraint and the linear inequality constraint together constitute the constraint of the optimization problem, and there are: /(I)
Using the convex hull properties of Bernstein-based polynomials, the constraints mentioned above can be modeled as linear equations and inequalities with respect to control points, giving a general form of the kth step programming problem.
And for the step S500, grouping unmanned aerial vehicles, and carrying out track planning on each group of unmanned aerial vehicles according to an objective function of a re-planned track, the first constraint, the second constraint and the third constraint in a mode of inter-group asynchronous planning and intra-group synchronous planning to obtain a target track.
The mode of asynchronous planning among groups and synchronous planning in groups is as follows: a plurality of unmanned aerial vehicles in the same group synchronously carry out track planning in the same time period; the ith group of unmanned aerial vehicles performs track planning in the ith time period, and the interval between two adjacent time periods is equal to or several times of the time of the track segmentation.
Referring to fig. 6, in particular, the unmanned system comprises 8 unmanned aerial vehicles, if trajectory planning is performed only in a synchronized manner; then at the time T1, 8 unmanned aerial vehicles simultaneously carry out track planning; at the time T2, 8 unmanned aerial vehicles simultaneously carry out track planning; at time T3, 8 drones simultaneously track planned.
According to the asynchronous planning mode between groups and the synchronous planning mode in groups, 8 unmanned aerial vehicles are divided into 4 groups, and each group of unmanned aerial vehicles comprises 2 unmanned aerial vehicles.
Track planning is carried out on the first group of unmanned aerial vehicles at the moment T1, track planning is carried out on the second group of unmanned aerial vehicles at the moment T2, track planning is carried out on the third group of unmanned aerial vehicles at the moment T3, track planning is carried out on the fourth group of unmanned aerial vehicles at the moment T4, the parallel synchronization is carried out in the groups, and the serial asynchronization is carried out between the groups. The second group of unmanned aerial vehicle planned tracks need to avoid the first group of tracks, the third group of unmanned aerial vehicle planned tracks avoid the first group of tracks and the second group of tracks, and the fourth group of unmanned aerial vehicle planned tracks avoid the first group of tracks, the second group of tracks and the third group of tracks.
The real tracks obtained are avoided among groups, the real tracks are not predicted, the global optimal solution is more approximate, and the robustness is high. And the inter-group asynchronism can realize time-sharing solution, and the load is uniformly spread, so that the instantaneous resources are saved.
In the simulation experiment, 8 unmanned aerial vehicles are divided into 4 groups, each group of 2 unmanned aerial vehicles, the average single planning time of the unmanned aerial vehicles is 10.74ms, the planned re-planning period of the unmanned aerial vehicles is 0.4s, the time required by each re-planning is 21.48ms, and the time required by each re-planning of synchronous planning without adopting a grouping asynchronous planning strategy is 85.92ms. The time required is shorter in an inter-group asynchronous programming and an intra-group synchronous programming manner than in a synchronous manner.
Referring to fig. 8, the unmanned aerial vehicle of the unmanned aerial vehicle system may be a Crazyfile micro unmanned aerial vehicle, and an embedded single-chip microcomputer with a Stm32 core-m kernel on the body. And obtaining pose information of the unmanned aerial vehicle through the dynamic capturing system. And the track planner of the ground computer carries out parallel multi-core solving of the track according to the distributed online multi-machine track planning method to obtain an optimal track, and the SE controller obtains the pose control quantity of the unmanned aerial vehicle according to the optimal track. And sending the pose control quantity to the unmanned aerial vehicle for execution.
Referring to fig. 7, in each re-planning period, an objective function and a constraint term are established for each unmanned aerial vehicle to construct an optimization problem, and a CPLEX solver is used for solving, so that trajectories of 8 unmanned aerial vehicles are obtained. The black sphere and the line segment correspond to the motion trail of dynamic obstacles in the environment, and other objects and curves correspond to the starting points and trail of 8 unmanned aerial vehicles. The distributed online multi-machine track planning method can not only finish the starting point task without collision, but also avoid unknown dynamic obstacles in the environment.
Another embodiment of the present application provides a computer-readable storage medium. The computer readable storage medium stores program instructions that when executed by the processor implement the distributed online multi-machine trajectory planning method as described above.
Another embodiment of the present application provides a distributed online multi-machine trajectory planning system. The unmanned aerial vehicle formation control system includes: a computer device; the computer apparatus includes a computer readable storage medium as described above.
Those of ordinary skill in the art will appreciate that all or some of the steps, systems, and methods disclosed above may be implemented as software, firmware, hardware, and suitable combinations thereof. Some or all of the physical components may be implemented as software executed by a processor, such as a central processing unit, digital signal processor, or microprocessor, or as hardware, or as an integrated circuit, such as an application specific integrated circuit. Such software may be distributed on computer readable media, which may include computer storage media (or non-transitory media) and communication media (or transitory media). The term computer storage media includes both volatile and nonvolatile, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data, as known to those skilled in the art. Computer storage media includes, but is not limited to, RAM, ROM, EEPROM, flash memory or other memory technology, CD-ROM, digital Versatile Disks (DVD) or other optical disk storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other medium which can be used to store the desired information and which can be accessed by a computer. Furthermore, as is well known to those of ordinary skill in the art, communication media typically embodies computer readable instructions, data structures, program modules or other data in a modulated data signal such as a carrier wave or other transport mechanism and includes any information delivery media. In the foregoing description of the present specification, reference has been made to the terms "one embodiment/example", "another embodiment/example", "certain embodiments/examples", and the like, means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the application. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiments or examples. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
Those of ordinary skill in the art will appreciate that all or some of the steps of the methods, systems, functional modules/units in the devices disclosed above may be implemented as software, firmware, hardware, and suitable combinations thereof.
The units described above as separate components may or may not be physically separate, and components shown as units may or may not be physical units, may be located in one place, or may be distributed over a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
In addition, each functional unit in the embodiments of the present application may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The integrated units may be implemented in hardware or in software functional units.
The integrated units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application may be embodied in essence or a part contributing to the prior art or all or part of the technical solution in the form of a software product stored in a storage medium, including multiple instructions to cause a computer device (which may be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method of the various embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (Random Access Memory RAM), a magnetic disk, or an optical disk, or other various media capable of storing a program.
In the several embodiments provided by the present application, it should be understood that the disclosed apparatus and method may be implemented in other manners. For example, the above-described apparatus embodiments are merely illustrative, and for example, the above-described division of units is merely a logical function division, and there may be another division manner in actual implementation, for example, a plurality of units or components may be combined or may be integrated into another system, or some features may be omitted, or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed with each other may be an indirect coupling or communication connection via some interfaces, devices or units, which may be in electrical, mechanical or other form. While embodiments of the present application have been shown and described, it will be understood by those of ordinary skill in the art that: many changes, modifications, substitutions and variations may be made to the embodiments without departing from the spirit and principles of the application, the scope of which is defined by the claims and their equivalents.
The present invention is not limited to the above embodiments, but can be modified, equivalent, improved, etc. by the same means to achieve the technical effects of the present invention, which are included in the spirit and principle of the present disclosure. Are intended to fall within the scope of the present invention. Various modifications and variations are possible in the technical solution and/or in the embodiments within the scope of the invention.
Claims (10)
1. The distributed online multi-machine track planning method is characterized by comprising the following steps of:
constructing a re-planning track of the unmanned aerial vehicle, wherein the re-planning track comprises track segments of all time periods, and the track segments comprise at least one control point;
obtaining the maximum flight distance of the unmanned aerial vehicle according to the maximum speed of the unmanned aerial vehicle, obtaining a predicted boundary according to the maximum flight distance, constructing a safety flight corridor of the unmanned aerial vehicle according to the predicted boundary and the coordinates of the static obstacle, and obtaining a first constraint according to the safety flight corridor;
predicting a priori trajectory according to the speed of the unmanned aerial vehicle, establishing collision constraint between the unmanned aerial vehicles according to the radius of the unmanned aerial vehicle, and simplifying the collision constraint between the unmanned aerial vehicles into a second constraint of convex constraint according to the priori trajectory;
Obtaining a maximum flight spherical region of the unmanned aerial vehicle according to the maximum speed of the unmanned aerial vehicle, obtaining a predicted track of the dynamic obstacle according to the speed of the dynamic obstacle, obtaining a track prediction error of the dynamic obstacle according to the maximum acceleration of the dynamic obstacle, and obtaining a third constraint according to the radius of the unmanned aerial vehicle, the radius of the dynamic obstacle and the track prediction error of the dynamic obstacle when the predicted track of the dynamic obstacle is in the maximum flight spherical region of the unmanned aerial vehicle;
Grouping unmanned aerial vehicles, and carrying out track planning on each group of unmanned aerial vehicles according to an objective function of a re-planned track, the first constraint, the second constraint and the third constraint in a mode of inter-group asynchronous planning and intra-group synchronous planning to obtain a target track.
2. The method of claim 1, wherein the re-planned trajectory of the ith unmanned aerial vehicle is expressed as: wherein p i (t) is a rescheduled track; b l,n is Bernstein-based polynomial; n is the order of the polynomial; /(I) Τ m is the normalized value of time t; /(I)An ith control point segmented for an ith track of the ith drone.
3. The method of claim 1, wherein the constructing a safe flight corridor of the unmanned aerial vehicle according to the estimated boundary and the coordinates of the static obstacle comprises:
Performing expansion operation from the starting point coordinates to a preset direction until an expansion area reaches an estimated boundary, the coordinates of a static obstacle or a map boundary, and obtaining a safety flight corridor planned for the first time;
the safe flight corridor for the a-th plan is expressed as: Wherein M is the number of segments of the track segment, k is the number of steps of the reprofiling,/> For the ith unmanned aerial vehicle to re-plan the safety flight corridor of the mth section in the kth step,For the ith unmanned plane, in the (m+1) -th section of the safety flight corridor re-planned in the (k-1) -th step, S is the expansion operation until the expansion area reaches the estimated boundary, the coordinates of the static obstacle or the map boundary, and S i is the initial coordinate,/>Is the predicted control point.
4. The distributed online multi-machine trajectory planning method of claim 1, wherein the prior trajectory is represented as: Wherein/> For a priori trajectory, M is the number of segments of the trajectory segment, k is the number of steps for the reprofiling, s i is the starting coordinate,/>Is the speed of the unmanned aerial vehicle, t is time,/>Predictive control point for the ith section of the ith drone re-planned at the kth step,/>Is the predicted speed.
5. The distributed online multi-machine trajectory planning method of claim 1, wherein the second constraint is expressed as: Wherein/> P i (t) is the re-planned trajectory of the ith unmanned aerial vehicle,/>For the i-th unmanned aerial vehicle's a priori trajectory,/>For the prior trajectory of the jth drone, r i is the radius of the ith drone, and r j is the radius of the jth drone.
6. The distributed online multi-machine trajectory planning method of claim 1, wherein the third constraint is expressed as: Wherein, p i (t) is the re-planning track of the ith unmanned aerial vehicle,/> R i is the radius of the ith unmanned aerial vehicle, r obs is the radius of the dynamic obstacle,Is the trajectory prediction error of the dynamic obstacle.
7. The distributed online multi-machine trajectory planning method of claim 2, wherein the objective function of the re-planned trajectory is: j=j d+Je, J is an objective function, J d is an energy objective function, and J e is an error objective function; w d is the weight parameter of the energy objective function, w e is the error objective function, T 0 is the start time, T M is the end time, phi is the derivative order, M is the number of segments of the track segment, k is the number of steps of the programming,/>And (3) the control point is the first control point of the mth track segment of the ith unmanned aerial vehicle in the kth step re-planning.
8. The method for distributed online multi-machine trajectory planning according to claim 1, wherein the inter-group asynchronous planning and intra-group synchronous planning are as follows:
a plurality of unmanned aerial vehicles in the same group synchronously carry out track planning in the same time period;
The ith group of unmanned aerial vehicles performs track planning in the ith time period, and the interval between two adjacent time periods is equal to or several times of the time of the track segmentation.
9. A computer readable storage medium, storing program instructions that when executed by a processor implement the distributed online multi-machine trajectory planning of any one of claims 1 to 8.
10. The distributed online multi-machine track planning system is characterized by comprising:
Computer means comprising a computer readable storage medium according to claim 9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410492778.2A CN118192608A (en) | 2024-04-23 | 2024-04-23 | Distributed online multi-machine track planning method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410492778.2A CN118192608A (en) | 2024-04-23 | 2024-04-23 | Distributed online multi-machine track planning method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN118192608A true CN118192608A (en) | 2024-06-14 |
Family
ID=91406262
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410492778.2A Pending CN118192608A (en) | 2024-04-23 | 2024-04-23 | Distributed online multi-machine track planning method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118192608A (en) |
-
2024
- 2024-04-23 CN CN202410492778.2A patent/CN118192608A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Alonso-Mora et al. | Distributed multi-robot formation control in dynamic environments | |
| Zhu et al. | Multi-robot flocking control based on deep reinforcement learning | |
| CN111562797A (en) | Unmanned aerial vehicle flight time optimal real-time trajectory optimization method capable of ensuring convergence | |
| Hafez et al. | Cooperative task assignment and trajectory planning of unmanned systems via HFLC and PSO | |
| CN113156954B (en) | Multi-agent cluster obstacle avoidance method based on reinforcement learning | |
| CN113900449B (en) | Multi-unmanned aerial vehicle track planning method and device, unmanned aerial vehicle and storage medium | |
| Li et al. | Autonomous waypoints planning and trajectory generation for multi-rotor UAVs | |
| Wang et al. | Disturbance observer-based backstepping formation control of multiple quadrotors with asymmetric output error constraints | |
| CN112650306A (en) | Unmanned aerial vehicle motion planning method based on dynamics RRT | |
| CN115903895A (en) | Trajectory planning method, device and system for unmanned aerial vehicle cluster | |
| CN115576353A (en) | Aircraft formation control method based on deep reinforcement learning | |
| Barros et al. | Using soft actor-critic for low-level UAV control | |
| WO2023010828A1 (en) | Lateral control of driverless device | |
| Yan et al. | Dynamic formation control for autonomous underwater vehicles | |
| CN118192608A (en) | Distributed online multi-machine track planning method and system | |
| Pan et al. | D 2 WA:“dynamic” DWA for motion planning of mobile robots in dynamic environments | |
| Wu et al. | Formation control strategy of multi-agent system with improved probabilistic roadmap method applied in restricted environment | |
| CN114167896B (en) | Unmanned aerial vehicle cluster control method for artificial potential energy field based on state prediction | |
| CN117075636A (en) | Control method for affine formation of multiple unmanned aerial vehicles | |
| Li et al. | Distributed differential game for formation control of multi-uav with obstacle avoidance | |
| Wang et al. | Research on Multi-Agent Task Allocation and Path Planning Based on Pri-MADDPG | |
| Chen et al. | A deep reinforcement learning approach for quadrotor path planning with search-based planner optimization | |
| Zhu et al. | Decentralized Control Method for UAV Arriving Simultaneously Based on Large Data Analysis | |
| CN117826828A (en) | Cluster target tracking method based on cubic curve | |
| CN117724481A (en) | Group learning control method and device for multiple unmanned vehicles based on self-adaptive neural network |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination |