Distributed online multi-machine trajectory planning method and system

Abstract

The present invention relates to a distributed online multi-machine trajectory planning method and system. The method comprises the following steps: constructing a replanned trajectory of an unmanned aerial vehicle (UAV); constructing a safe flight corridor for the UAV according to the estimated boundary and the coordinates of the static obstacles, thereby obtaining a first constraint for the UAV to avoid static obstacles; simplifying the collision constraint between the UAVs into a second constraint for obstacle avoidance between the UAVs, which is a convex constraint, according to the prior trajectory; obtaining a third constraint for the UAV to avoid dynamic obstacles according to the radius of the UAV, the radius of the dynamic obstacle and the trajectory prediction error of the dynamic obstacle as needed; grouping the UAVs, and performing trajectory planning for each group of UAVs according to the objective function of the replanned trajectory, the first constraint, the second constraint and the third constraint in a manner of asynchronous planning between groups and synchronous planning within groups, thereby obtaining a target trajectory. The method improves the solution speed of the trajectory planning, realizes time-sharing solution, distributes the load and saves instantaneous resources.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicle (UAV) control, and in particular relates to a distributed online multi-machine trajectory planning method and system. Background Art

[0002] Drones are a branch of robotics with a wide range of applications. Due to limitations in size, payload, and energy, a single drone cannot meet the demands of complex missions. Therefore, multi-drone collaboration is currently the primary application direction.

[0003] Given a set of drones with known initial and target locations, the problem of drone trajectory planning is to find a set of continuous functions that move each drone from its starting position to its target location. Since many drones share the same space, the planning algorithm must take into account the environment and the states of neighboring drones to avoid collisions.

[0004] The earliest developed techniques for continuous multi-UAV trajectory optimization are Mixed Integer Linear Programming (MILP) and Mixed Integer Quadratic Programming (MIQP). However, these methods jointly optimize the trajectories of all UAVs. While they guarantee optimality, their time complexity limits their application to scenarios with a small number of obstacles and UAVs. Decoupling collision constraints between UAVs using sequential programming significantly improves computational efficiency, but can lead to the loss of feasible solutions. Distributed and reactive approaches include Buffer Voronoi Cells (BVC), Velocity Obstacles (VO), and Optimal Reciprocal Collision Avoidance (ORCA), but none of these methods optimize the target trajectory. Multi-agent offline trajectory generation algorithms based on distributed model predictive control (DMPC) generally describe trajectories based on discrete time points. This suffers from the large number of optimization variables, slow solution speed, and lack of safety assurance between discrete points.

[0005] Meeting the requirements of diverse applications requires a more generalized definition of the planning problem, discarding potential optimizations for specific problems that could improve optimality and reduce computational time. Improving trajectory optimality requires more complex and sophisticated modeling of the problem and more iterations in the feasible space, which inevitably comes at the cost of increased planning time. These issues increase the time it takes to solve the drone's trajectory and reduce its efficiency. Summary of the Invention

[0006] Aiming to solve at least one of the technical problems existing in the prior art, the present invention provides a distributed online multi-machine trajectory planning method and system, which improves the efficiency of trajectory solving.

[0007] An embodiment of the first aspect of the present invention, a distributed online multi-machine trajectory planning method, comprises the following steps:

[0008] Constructing a replanned trajectory of the UAV, wherein the replanned trajectory includes trajectory segments for each time period, and each trajectory segment includes at least one control point;

[0009] Obtaining a maximum flight distance of the drone based on the maximum speed of the drone, obtaining an estimated boundary based on the maximum flight distance, constructing a safe flight corridor for the drone based on the estimated boundary and coordinates of static obstacles, and obtaining a first constraint based on the safe flight corridor;

[0010] Predicting a priori trajectories based on the velocities of the UAVs, establishing collision constraints between the UAVs based on their radii, and simplifying the collision constraints between the UAVs into a second constraint that is a convex constraint based on the priori trajectories;

[0011] The maximum flight spherical area of ​​the UAV is obtained based on the maximum speed of the UAV, the predicted trajectory of the dynamic obstacle is obtained based on the speed of the dynamic obstacle, and the trajectory prediction error of the dynamic obstacle is obtained based on the maximum acceleration of the dynamic obstacle. When the predicted trajectory of the dynamic obstacle is within the maximum flight spherical area of ​​the UAV, the third constraint is obtained based on the radius of the UAV, the radius of the dynamic obstacle, and the trajectory prediction error of the dynamic obstacle.

[0012] The drones are grouped, and trajectory planning is performed on each group of drones according to the first constraint, the second constraint, and the third constraint in a manner of asynchronous planning between groups and synchronous planning within groups to obtain a target trajectory.

[0013] According to certain embodiments of the first aspect of the present invention, the replanned trajectory of the i-th UAV is expressed as: Among them, p i (t) is the replanned trajectory; B l,n is the Bernstein basis polynomial; n is the order of the polynomial; τm is the normalized value at time t; is the lth control point of the mth trajectory segment of the i-th UAV.

[0014] According to certain embodiments of the first aspect of the present invention, constructing a safe flight corridor for the drone based on the estimated boundary and the coordinates of the static obstacles includes:

[0015] Expand from the starting coordinates in the preset direction until the expanded area reaches the estimated boundary, the coordinates of the static obstacle, or the map boundary, to obtain the initially planned safe flight corridor;

[0016] The safe flight corridor planned for the ath time is expressed as: Among them, M is the number of segments of the trajectory, k is the number of steps of replanning, For the safe flight corridor of the mth segment replanned by the i-th UAV in the k-th step, is the safe flight corridor of the m+1th segment replanned by the i-th UAV in the k-1th step, S is the expansion operation until the expansion area reaches the estimated boundary, the coordinates of the static obstacle or the map boundary, s i is the starting coordinate, is the control point for prediction.

[0017] According to some embodiments of the first aspect of the present invention, the a priori trajectory is expressed as: in, is the prior trajectory, M is the number of segments of the trajectory, k is the number of steps for replanning, s i is the starting coordinate, is the speed of the UAV, t is the time, is the predicted control point of the mth segment of the i-th UAV replanned in the k-th step, Forecast speed.

[0018] According to some embodiments of the first aspect of the present invention, the second constraint is expressed as: in, p i (t) is the replanned trajectory of the i-th UAV, is the prior trajectory of the i-th UAV, is the prior trajectory of the j-th UAV, r i is the radius of the i-th UAV, r j is the radius of the j-th UAV.

[0019] According to some embodiments of the first aspect of the present invention, the third constraint is expressed as: Among them, p i(t) is the replanned trajectory of the i-th UAV, is the predicted trajectory of the dynamic obstacle, r i is the radius of the i-th UAV, r obs is the radius of the dynamic obstacle, is the trajectory prediction error of the dynamic obstacle.

[0020] According to some embodiments of the first aspect of the present invention, the objective function of the replanned trajectory is: J = J d +J e , J is the objective function, J d is the energy objective function, J e is the error objective function; w d is the weight parameter of the energy objective function, w e is the error objective function, T0 is the starting time, T M is the termination time, φ is the derivative order, M is the number of segments of the trajectory, k is the number of steps for replanning, is the lth control point of the mth trajectory segment of the i-th UAV in the k-th replanning step.

[0021] According to certain embodiments of the first aspect of the present invention, the method of asynchronous planning between groups and synchronous planning within a group is:

[0022] Multiple drones in the same group perform trajectory planning synchronously in the same time period;

[0023] The i-th group of UAVs performs trajectory planning in the i-th time period, and the interval between two adjacent time periods is equal to or several times the time of trajectory segmentation.

[0024] An embodiment of the second aspect of the present application is a computer-readable storage medium storing program instructions, which, when executed by a processor, implement the distributed online multi-machine trajectory planning as described above.

[0025] An embodiment of the third aspect of the present application is a distributed online multi-machine trajectory planning system, comprising: a computer device, wherein the computer device comprises a computer-readable storage medium as described above.

[0026] The beneficial effects of the present invention include: constructing a replanned trajectory of the UAV; constructing a safe flight corridor for the UAV based on the estimated boundary and the coordinates of the static obstacles to obtain the first constraint for the UAV to avoid static obstacles; simplifying the collision constraint between UAVs into a second constraint for obstacle avoidance between UAVs that is a convex constraint based on the prior trajectory; obtaining the third constraint for the UAV to avoid dynamic obstacles based on the radius of the UAV, the radius of the dynamic obstacle and the trajectory prediction error of the dynamic obstacle as needed; grouping the UAVs, and performing trajectory planning for each group of UAVs based on the objective function of the replanned trajectory, the first constraint, the second constraint and the third constraint in a manner of asynchronous planning between groups and synchronous planning within the group to obtain the target trajectory; improving the solution speed of trajectory planning, realizing time-sharing solution, sharing the load and saving instantaneous resources.

[0027] In addition, additional aspects and advantages of the present invention will be set forth in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 It is a step diagram of the distributed online multi-machine trajectory planning method;

[0029] Figure 2 is a flowchart of the improved safe flight corridor construction algorithm;

[0030] Figure 3 It is a schematic diagram of the obstacle avoidance constraints of the UAV and static obstacles;

[0031] Figure 4 It is the geometric meaning diagram of obstacle avoidance constraints between UAVs;

[0032] Figure 5 It is a schematic diagram of the obstacle avoidance constraints between UAVs;

[0033] Figure 6 This is a schematic diagram of timestamps with asynchronous planning between groups and synchronous planning within a group;

[0034] Figure 7 This is the simulation result diagram of the distributed online multi-machine trajectory planning method;

[0035] Figure 8 It is a structural diagram of the UAV system and the ground computer. DETAILED DESCRIPTION

[0036] The following will be combined with the embodiments and drawings to clearly and completely describe the concept, specific structure and technical effects of the present invention so as to fully understand the purpose, scheme and effect of the present invention. It should be noted that the embodiments and features in the embodiments of this application can be combined with each other unless there is a conflict.

[0037] 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 fixed or connected to the other feature or indirectly fixed or connected to the other feature. Furthermore, terms such as "upper," "lower," "left," "right," "top," and "bottom" used in this disclosure are intended solely to describe the relative positions of the components of the disclosure as shown in the accompanying drawings.

[0038] In addition, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art. The terms used in this specification are only for describing specific embodiments and are not intended to limit the present invention. The term "and / or" as used herein includes any combination of one or more of the related listed items.

[0039] For autonomous drones, trajectory planning often serves as a downstream component of real-time perception, requiring real-time online replanning based on updated perception information. Online replanning places high demands on planning time. Optimization problems that directly establish obstacle constraints and inter-drone constraints are nonlinear and non-convex, making them prone to slow solutions, difficulty converging, or convergence to local minima, resulting in poor flight performance.

[0040] An embodiment of the present invention provides a distributed online multi-machine trajectory planning method, which is applied to an unmanned aerial vehicle system.

[0041] Reference Figure 1 ,Distributed online multi-machine trajectory planning method, including the following steps:

[0042] Step S100, constructing a replanned trajectory of the UAV;

[0043] Step S200: obtaining a maximum flight distance of the UAV based on the maximum speed of the UAV, obtaining an estimated boundary based on the maximum flight distance, constructing a safe flight corridor for the UAV based on the estimated boundary and the coordinates of static obstacles, and obtaining a first constraint based on the safe flight corridor;

[0044] Step S300: predicting a priori trajectories based on the velocities of the UAVs, establishing collision constraints between the UAVs based on their radii, and simplifying the collision constraints between the UAVs into a second constraint that is a convex constraint based on the priori trajectories;

[0045] Step S400: Determine the maximum spherical flight area of ​​the UAV based on the maximum speed of the UAV, determine the predicted trajectory of the dynamic obstacle based on the speed of the dynamic obstacle, and determine the trajectory prediction error of the dynamic obstacle based on the maximum acceleration of the dynamic obstacle. When the predicted trajectory of the dynamic obstacle is within the maximum spherical flight area of ​​the UAV, determine the third constraint based on the radius of the UAV, the radius of the dynamic obstacle, and the trajectory prediction error of the dynamic obstacle.

[0046] In step S500, the drones are grouped and trajectory planning is performed on each group of drones according to the objective function, the first constraint, the second constraint, and the third constraint of the replanned trajectory in a manner of asynchronous planning between groups and synchronous planning within groups to obtain the target trajectory.

[0047] In step S100, a replanned trajectory of the UAV is constructed; the replanned trajectory includes trajectory segments of each time period, and the trajectory segment includes at least one control point.

[0048] The replanned trajectory of the i-th UAV at the k-th replanning step is expressed as: Among them, p i (t) is the replanned trajectory; B l,n is the Bernstein basis polynomial; n is the order of the polynomial; τ m is the normalized value at time t; is the lth control point of the mth trajectory segment of the i-th UAV.

[0049] For the objective function of replanning trajectory, in order to ensure that the trajectory is smooth and continuous, the energy of the trajectory is minimized. The integral of the square of the second norm of the order derivative in the replanning domain is used as the energy cost and given a weight w d , energy objective function J d Expressed as: Among them, J d is the energy objective function, w d is the weight parameter of the energy objective function, T0 is the starting time, T M is the termination time, φ is the derivative order, is the lth control point of the mth trajectory segment of the i-th UAV in the k-th replanning step.

[0050] Since online replanning is a rolling optimization problem within a certain time domain, and the target point is not imposed as a hard constraint, in order to make the trajectory reach the target point quickly, the sum of the squares of the distances from the drone's position to the end point in the next κ steps is minimized as the error cost, and the weight w is given. e , error objective function J e Expressed as: Among them, J e is the error objective function, w e is the error objective function, M is the number of trajectory segments, and k is the number of replanning steps.

[0051] Both objective function terms can be expressed as quadratic forms of control points. The energy cost and the error cost each have their own weight matrix, which are balanced by adjusting appropriate weight coefficients to avoid oscillation problems.

[0052] The total cost J of the objective function is: J = J d + J e .

[0053] For step S200, by virtue of the convex hull property of the control points of the Bernstein basis polynomial, the control points are constrained in the convex flight safety corridor SFC, which can ensure that the UAV does not collide with irregular obstacles in the environment.

[0054] The axis search algorithm expands in six directions successively in the positive x-axis direction, negative x-axis direction, positive y-axis direction, negative y-axis direction, positive z-axis direction, and negative z-axis direction at the point to be inflated until the coordinates of static obstacles or the map boundary are touched in each direction. However, the expansion process of the axis search algorithm takes a long time. In this embodiment, the construction of SFC is improved by virtue of the property of online replanning, and it is not necessary to search out all the feasible convex spaces in the environment. The axis search algorithm of SFC is improved by using the dynamic constraints of the UAV, and the expansion in this direction will stop when each axis of SFC extends to the dynamic limit area, obstacle area, or boundary area.

[0055] At the time of initial planning, the starting point coordinates s i are used as the initial SFC. According to the maximum speed of the UAV, the maximum flight distance of the UAV is obtained. The maximum flight distance is Δp = v max (1 + a)Δt; where a is an adjustment parameter. According to the maximum flight distance, the estimated boundary is obtained. According to the improved algorithm for constructing the safe flight corridor, inflation operations are performed from the starting point coordinates in the preset direction until the inflated area reaches the estimated boundary, the coordinates of static obstacles, or the map boundary, and the initially planned safe flight corridor is obtained.

[0056] Refer to Figure 2 , the improved algorithm for constructing the safe flight corridor can be implemented according to the following process: Initialize the number of segments 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. Determine whether m < M is satisfied. If so, initialize the m-th segment of SFC as the initial segment Determine whether the set D is empty. If it is, update the number of segments m = m + 1. If not, detect whether the SFC encounters an obstacle in the index e direction. If the SFC does not encounter an obstacle in the index e direction, determine whether the SFC has expanded to the boundary of the estimated position. If the SFC encounters an obstacle in the index e direction or the SFC expands to the boundary of the estimated position, remove the direction corresponding to the index e from the set D, and set E = E - 1, e = (e - 1 + E) % E. If the SFC has not expanded to the boundary of the estimated position, expand the SFC by one grid in the index e direction. Update the direction index e = (e + 1) % E. Update the number of segments m = m + 1, and return to the step of determining whether m < M is satisfied. When m is greater than or equal to M, end the process.

[0057] For non-first-time planning, the planning result of the previous step can be used as the prediction information for this planning, and the replanning period is set to be equal to or a multiple of the interval time between two segments of the piecewise polynomial. In the case of equality, the SFCs of the first to M segments constructed in the (k - 1)-th step can be directly applied to the 0 to M - 1 segments of the k-th step planning, without re-axis search to construct a new SFC, and it can be obtained directly by rolling. The SFC of the M-th segment trajectory without rolling prediction is constructed by an improved axis search algorithm to construct the SFC, and it is constructed by rolling in this way, which greatly reduces the time complexity of SFC construction.

[0058] Specifically, the safe flight corridor for the a-th planning is expressed as: where M is the number of segments of the trajectory, k is the number of replanning steps, is the safe flight corridor of the i-th UAV in the m-th segment of the k-th replanning, is the safe flight corridor of the i-th UAV in the (m + 1)-th segment of the (k - 1)-th replanning, S is the expansion operation until the expansion area reaches the estimated boundary, the coordinates of static obstacles or the map boundary, s i is the starting coordinate, is the predicted control point.

[0059] Obtain the first constraint for the UAV to avoid static obstacles according to the safe flight corridor.

[0060] For the method of directly constructing constraints based on the rolling SFC, the obstacle avoidance constraints of the k-th step cannot be directly obtained implicitly during the period when the SFC does not need to be re-searched. The method of improving the axis search to construct the SFC based on the replanning step size and dynamic constraints reduces the search expansion space and improves the efficiency. Therefore, in distributed online multi-UAV planning, there will be no problem of long calculation time like in offline multi-UAV planning when constructing SFC constraints.

[0061] Refer to Figure 3, consisting of multiple convex cuboid constraints. Simulation experiments show that in eight unplanned UAVs, the average time for constructing a single plan using the original SFC constraint was 1.38ms, while the average time for constructing a single plan using the improved SFC constraint was 0.9ms. Comparisons of the computational time for planning with and without the improved SFC for 2, 4, 8, and 16 UAVs show that the improved method improved time by 22.5% for 2 UAVs, 26% for 4 UAVs, 34.8% for 8 UAVs, and 35.2% for 16 UAVs.

[0062] In step S300, a priori trajectory is predicted according to the speed of the UAV, a collision constraint between the UAVs is established according to the radius of the UAV, and the collision constraint between the UAVs is simplified to a second constraint of a convex constraint according to the priori trajectory.

[0063] For the initial planning, the prior trajectory is predicted based on the UAV’s velocity.

[0064] The prior trajectory is expressed as: in, is the prior trajectory, M is the number of segments of the trajectory, k is the number of steps for replanning, s i is the starting coordinate, is the speed of the UAV, t is the time, is the predicted control point of the mth segment of the i-th UAV replanned in the k-th step, Forecast speed.

[0065] For the k-th step planning, the prior trajectory of the 0th to M-1th segment can be directly obtained from the 1st to Mth segments of the k-1th step, and the planning result of the previous step is used as the prior trajectory of other drones in this planning; and the prior trajectory of the Mth segment is obtained according to The prior trajectory is predicted.

[0066] In order to describe the obstacle avoidance constraints between drones, the mutual collision model between drones is first established as follows: Among them, r i and r j are the radii of the i-th and j-th drones, E=diag([1,1,1 / (c dw ) 2 ], c dw is the downwash effect coefficient of the drone. This is because the propeller wind force will affect the Z-axis direction of the drone during flight. Therefore, the collision distance in the Z-axis direction of the drone is expanded to c times the original distance. dw times.

[0067] It is further deduced that the condition for no collision between the i-th UAV and the j-th UAV is:

[0068]

[0069] Since the obstacle avoidance constraints between drones described above are in a non-convex space, directly applying these non-convex constraints will cause the optimization problem to fail to converge or be slow to solve. Therefore, it is necessary to convert it into a convex approximate feasible space. Using the predicted trajectories of other drones as prior information, a collision constraint between drones is established based on the radius of the drones. That is, the collision avoidance constraint between drones requires the i-th drone to avoid the j-th drone. The collision avoidance constraint between drones can be expressed as:

[0070] The collision constraint between the UAVs is simplified into a second constraint of a convex constraint according to the prior trajectory.

[0071] right Simplify, let Use f(x)=f(x0)+f (1) Taylor expansion of (x0)(x-x0)+o(x-x0) at x0 is performed, retaining the first-order linear terms, discarding the higher-order infinitesimals, and simplifying the non-convex quadratic constraint to a convex linear constraint, which yields:

[0072]

[0073] make Simplifying, we get the second constraint, which is expressed as: in, p i (t) is the replanned trajectory of the i-th UAV, is the prior trajectory of the i-th UAV, is the prior trajectory of the j-th UAV, r i is the radius of the i-th UAV, r j is the radius of the j-th UAV.

[0074] Reference Figure 4 Based on the direction of the minimum distance from the convex hull of the predicted trajectory to the center of the collision model, the normal vector and corresponding tangent plane are obtained. The normal vector is the direction of the point on the convex hull closest to the center of the sphere, and the tangent is the tangent plane at the intersection of the normal vector and the sphere. Geometrically, this normal vector and tangent plane form the collision constraint between machines, simplifying the non-convex feasible space into a convex half-space. To find the closest point on the convex hull to the center of the sphere, the GJK algorithm can be used to directly calculate the closest distance and direction vector between the two convex hulls.

[0075] Reference Figure 5, consisting of multiple linear half-space constraints. By using the predicted prior path to find the separating hyperplane and transform it into a convex linear space, the collision avoidance constraints between drones are simplified, transforming the collision avoidance constraints from the original non-convex quadratic equations to the current linear convex constraints, improving the efficiency of solving the optimization problem.

[0076] Step S400 is to construct constraints for the drone to avoid dynamic obstacles.

[0077] Establish the maximum range radius R that the drone can reach, and only perform collision detection with the predicted trajectory of dynamic obstacles within the maximum flight spherical area with a radius of R, reducing the amount of collision constraints in the optimization.

[0078] According to the UAV dynamics limit, the maximum radius R is obtained from the maximum speed of the UAV: ​​R = v max (T M -T0). The predicted trajectory of the dynamic obstacle is obtained based on its speed: According to the maximum acceleration of the dynamic obstacle, the trajectory prediction error of the dynamic obstacle is: b is an adjustable threshold parameter.

[0079] Assuming the dynamic obstacle is a sphere, the collision model between the drone and the dynamic obstacle can be obtained as follows:

[0080] When the predicted trajectory of a dynamic obstacle is within the maximum flight spherical area of ​​the UAV, the obstacle avoidance constraint between the UAV and the dynamic obstacle is obtained based on the radius of the UAV, the radius of the dynamic obstacle, and the trajectory prediction error of the dynamic obstacle, i.e., the third constraint. The third constraint is:

[0081] because is a quadratic polynomial with respect to time t, which can be expressed as a Bernstein basis polynomial. Therefore, for the constraints between the UAV and the dynamic obstacles, the Taylor expansion method at the predicted trajectory can also be used to convert the non-convex constraints into convex linear half-space constraints.

[0082] Regarding collision detection between drones and dynamic obstacles, the drone trajectories and dynamic obstacle trajectories are both Bernstein basis polynomials. Therefore, the convex hull property of the control points can be used to calculate the distance between the two simplexes using the GJK algorithm to determine whether the distance is greater than the safe distance, greatly improving the collision detection solution speed.

[0083] In addition, the trajectory needs to meet the initial conditions and match the current position, velocity, and acceleration of the drone, so the state constraints of the initial point need to be added.

[0084] Online replanning does not require adding hard constraints on the end state to the drone, forcing the trajectory to approach the end state exists as a cost in the objective function. The initial state constraint is:

[0085]

[0086] For smooth flight, the trajectory should be high-order continuous, and it is necessary to add continuity constraints of piecewise Bernstein basis polynomials.

[0087] The smoothness and continuity of the trajectory require that the piecewise Bernstein basis polynomials satisfy the following continuity constraints, namely: According to the properties of the derivatives of the Bernstein basis polynomials, the constraints on the control points are simplified as follows:

[0088] Since the control volume of the drone has its dynamic limitations, it is necessary to add maximum speed and maximum acceleration constraints. The maximum speed and maximum acceleration constraints are: Since the derivatives of the Bernstein basis polynomials are also Bernstein basis polynomials, the velocity curve and acceleration curve are also Bernstein basis polynomials, and they also have the property that the convex hull composed of control points surrounds the trajectory. Therefore, the control points of the velocity and acceleration curves can be directly constrained within the velocity and acceleration limits. The maximum velocity and maximum acceleration constraints are simplified to:

[0089] The state constraints and continuity constraints on the starting and target points can be constrained by linear equations A eq c=b eq Re-stating, the upper and lower limits of the control quantity can be constrained by linear inequality constraints A ieq c≤b ieq Restated, these constraints can all be expressed in the form of linear equations or inequalities on the control points of the decision variables, which are convex constraints on the optimization variables.

[0090] The objective function formula, obstacle avoidance constraint formula between drones, collision avoidance constraint formula between drones, initial state constraint formula, continuity constraint formula, and dynamics constraint constraint formula together form q n The online multi-machine trajectory optimization problem of UAVs is: d and J e are the energy cost and error cost of the objective function, respectively. is the set of decision variable control points, A eq and b eq A is the matrix parameter of the linear equality constraint in all the above constraints. ieq and b ieqis the matrix parameter of the linear inequality constraint in all the above constraints. The linear equality constraint and the linear inequality constraint together constitute the constraints of the optimization problem, which are:

[0091] By utilizing the convex hull properties of the Bernstein basis polynomials, the constraints mentioned above can be modeled as linear equations and inequalities about the control points, giving the general form of the k-th step planning problem.

[0092] In step S500, the drones are grouped, and trajectory planning is performed on each group of drones according to the objective function of the replanned trajectory, the first constraint, the second constraint, and the third constraint in a manner of asynchronous planning between groups and synchronous planning within groups to obtain a target trajectory.

[0093] The method of asynchronous planning between groups and synchronous planning within a group is as follows: multiple drones in the same group perform trajectory planning synchronously in the same time period; the i-th group of drones performs trajectory planning in the i-th time period, and the interval between two adjacent time periods is equal to or several times the time of the trajectory segmentation.

[0094] Reference Figure 6 Specifically, the UAV system includes 8 UAVs. If trajectory planning is performed only in a synchronous manner, then at time T1, the 8 UAVs perform trajectory planning simultaneously; at time T2, the 8 UAVs perform trajectory planning simultaneously; at time T3, the 8 UAVs perform trajectory planning simultaneously... and so on.

[0095] According to the method of asynchronous planning between groups and synchronous planning within groups, the 8 drones are divided into 4 groups, and each group includes 2 drones.

[0096] Trajectory planning is performed for the first group of drones at time T1, for the second group at time T2, for the third group at time T3, and for the fourth group at time T4. The planning is performed in parallel and synchronously within each group, but serially and asynchronously between groups. The trajectory planned for the second group of drones avoids the trajectory of the first group, the trajectory planned for the third group avoids the trajectory of the first and second groups, and the trajectory planned for the fourth group avoids the trajectory of the first, second, and third groups.

[0097] The inter-group avoidance is based on the actual trajectory obtained, rather than the predicted trajectory, which is closer to the global optimal solution and has strong robustness. Furthermore, the inter-group asynchrony enables time-sharing solution, which evenly distributes the load and saves instantaneous resources.

[0098] In a simulation experiment, eight drones were divided into four groups of two each. The average planning time for each drone was 10.74ms. With a replanning cycle of 0.4s, each replanning took 21.48ms. Synchronous planning without group asynchronous planning took 85.92ms per replanning. Asynchronous planning between groups and synchronous planning within groups achieves significantly faster replanning times than the synchronous approach.

[0099] Reference Figure 8 The drone system can use a Crazyfile micro-drone with an embedded MCU based on an STM32 Cortex-M core. The drone's position and posture information is obtained through a motion capture system. The ground computer's trajectory planner uses the aforementioned distributed online multi-machine trajectory planning method to perform a multi-core parallel calculation to obtain the optimal trajectory. The SE controller generates the drone's position and posture control variables based on the optimal trajectory and sends these control variables to the drone for execution.

[0100] Reference Figure 7 In each replanning cycle, the objective function and constraints for each drone are established to construct an optimization problem. This problem is then solved using the CPLEX solver, yielding the trajectories of the eight drones. The black spheres and line segments correspond to the motion trajectories of dynamic obstacles in the environment, while the other objects and curves correspond to the starting points and trajectories of the eight drones. This demonstrates that this distributed online multi-drone trajectory planning method not only achieves collision-free completion of the starting point task but also avoids unknown dynamic obstacles in the environment.

[0101] Another embodiment of the present application provides a computer-readable storage medium having program instructions stored therein, which, when executed by a processor, implements the distributed online multi-machine trajectory planning method described above.

[0102] Another embodiment of the present application provides a distributed online multi-machine trajectory planning system. The UAV formation control system includes: a computer device; the computer device includes the computer-readable storage medium described above.

[0103] Those skilled in the art will appreciate that all or some of the steps and systems in the method disclosed above can be implemented as software, firmware, hardware, and appropriate combinations thereof. Some physical components or all physical components can be implemented as software executed by a processor, such as a central processing unit, a digital signal processor, or a microprocessor, or implemented as hardware, or implemented as an integrated circuit, such as an application-specific integrated circuit. Such software can be distributed on a computer-readable medium, and the computer-readable medium can include computer storage media (or non-transitory media) and communication media (or temporary media). As known to those skilled in the art, the term computer storage media is included in any method or technology for storing information (such as computer-readable instructions, data structures, program modules, or other data) and is volatile and non-volatile, removable, and non-removable. 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 tapes, disk storage, or other magnetic storage devices, or any other medium that can be used to store desired information and can be accessed by a computer. In addition, it is well known to those skilled in the art that communication media generally contain computer-readable instructions, data structures, program modules, or other data in a modulated data signal such as a carrier wave or other transmission mechanism, and may include any information delivery medium. In the above description of this specification, the reference terms "one embodiment / example", "another embodiment / example" or "certain embodiments / examples" and the like are intended to mean that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0104] Those skilled in the art will appreciate that all or some of the steps in the methods, systems, and functional modules / units in the devices disclosed above may be implemented as software, firmware, hardware, or appropriate combinations thereof.

[0105] The units described above as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of these units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0106] In addition, the functional units in the various embodiments of the present application may be integrated into a single processing unit, or each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0107] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, which is stored in a storage medium and includes multiple instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of various embodiments of the present application. The aforementioned storage medium includes: various media that can store programs, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0108] In the several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely schematic. For example, the division of the above-mentioned units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms. Although the embodiments of the present application have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and purpose of the present application, and the scope of the present application is defined by the claims and their equivalents.

[0109] The above description is merely a preferred embodiment of the present invention. The present invention is not limited to the aforementioned embodiments. As long as the technical effects of the present invention are achieved by the same means, any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present disclosure shall be included within the scope of protection of the present disclosure. Within the scope of protection of the present invention, its technical solutions and / or implementation methods may be modified and varied in various ways.

Claims

1. Distributed online multi-machine trajectory planning method, characterized by: The following steps are involved: Constructing a replanned trajectory of the UAV, wherein the replanned trajectory includes trajectory segments for each time period, and each trajectory segment includes at least one control point; Obtaining a maximum flight distance of the drone based on the maximum speed of the drone, obtaining an estimated boundary based on the maximum flight distance, constructing a safe flight corridor for the drone based on the estimated boundary and coordinates of static obstacles, and obtaining a first constraint based on the safe flight corridor; Predicting a priori trajectories based on the velocities of the UAVs, establishing collision constraints between the UAVs based on their radii, and simplifying the collision constraints between the UAVs into a second constraint that is a convex constraint based on the priori trajectories; The maximum flight spherical area of ​​the UAV is obtained based on the maximum speed of the UAV, the predicted trajectory of the dynamic obstacle is obtained based on the speed of the dynamic obstacle, and the trajectory prediction error of the dynamic obstacle is obtained based on the maximum acceleration of the dynamic obstacle. When the predicted trajectory of the dynamic obstacle is within the maximum flight spherical area of ​​the UAV, the third constraint is obtained based on the radius of the UAV, the radius of the dynamic obstacle, and the trajectory prediction error of the dynamic obstacle. The drones are grouped, and trajectory planning is performed on each group of drones according to the objective function of the replanned trajectory, the first constraint, the second constraint, and the third constraint in a manner of asynchronous planning between groups and synchronous planning within groups to obtain a target trajectory.

2. The distributed online multi-machine trajectory planning method according to claim 1, characterized in that: The replanned trajectory of the i-th UAV is expressed as: Among them, p i (t) is the replanned trajectory; B l,n is the Bernstein basis polynomial; n is the order of the polynomial; τ m is the normalized value at time t; is the lth control point of the mth trajectory segment of the i-th UAV.

3. The distributed online multi-machine trajectory planning method according to claim 1, characterized in that: The step of constructing a safe flight corridor for the UAV based on the estimated boundary and the coordinates of the static obstacles includes: Expand from the starting coordinates in the preset direction until the expanded area reaches the estimated boundary, the coordinates of the static obstacle, or the map boundary, to obtain the initially planned safe flight corridor; The safe flight corridor planned for the ath time is expressed as: Among them, M is the number of segments of the trajectory, k is the number of steps of replanning, For the safe flight corridor of the mth segment replanned by the i-th UAV in the k-th step, is the safe flight corridor of the m+1th segment replanned by the i-th UAV in the k-1th step, S is the expansion operation until the expansion area reaches the estimated boundary, the coordinates of the static obstacle or the map boundary, s i is the starting coordinate, is the control point for prediction.

4. The distributed online multi-machine trajectory planning method according to claim 1, characterized in that: The prior trajectory is expressed as: in, is the prior trajectory, M is the number of segments of the trajectory, k is the number of steps for replanning, s i is the starting coordinate, is the speed of the UAV, t is the time, is the predicted control point of the mth segment of the i-th UAV replanned in the k-th step, Forecast speed.

5. The distributed online multi-machine trajectory planning method according to claim 1, characterized in that: The second constraint is expressed as: in, p i (t) is the replanned trajectory of the i-th UAV, is the prior trajectory of the i-th UAV, is the prior trajectory of the j-th UAV, r i is the radius of the i-th UAV, r j is the radius of the j-th UAV.

6. The distributed online multi-machine trajectory planning method according to claim 1, characterized in that: The third constraint is expressed as: Among them, p i (t) is the replanned trajectory of the i-th UAV, is the predicted trajectory of the dynamic obstacle, r i is the radius of the i-th UAV, 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 according to claim 2, characterized in that: The objective function of the replanned trajectory is: J = J d +J e , J is the objective function, J d is the energy objective function, J e is the error objective function; w d is the weight parameter of the energy objective function, w e is the error objective function, T0 is the starting time, T M is the termination time, φ is the derivative order, M is the number of segments of the trajectory, k is the number of steps for replanning, is the lth control point of the mth trajectory segment of the i-th UAV in the k-th replanning step.

8. The distributed online multi-machine trajectory planning method according to claim 1, characterized in that: The method of asynchronous planning between groups and synchronous planning within a group is: Multiple drones in the same group perform trajectory planning synchronously in the same time period; The i-th group of UAVs performs trajectory planning in the i-th time period, and the interval between two adjacent time periods is equal to or several times the time of trajectory segmentation.

9. A computer-readable storage medium, characterized in that Program instructions are stored, and when the program instructions are executed by a processor, the distributed online multi-machine trajectory planning according to any one of claims 1 to 8 is implemented.

10. Distributed online multi-machine trajectory planning system, characterized by: include: A computer device comprising the computer-readable storage medium according to claim 9.

Images