Cooperative trajectory optimization method and system based on undirected graph formation measurement
Through the undirected graph formation measurement and B-spline optimization model, the smooth continuity problem of multi-air drone trajectory under multi-constraint conditions is solved, and the trajectory adjustment under the formation requirements is realized to meet dynamics and safety requirements. .
Patent Information
- Application Number
- CN202411204687.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-30
- Publication Date
- 2024-11-22
AI Technical Summary
Under the multi-constraint conditions such as formation, safety, and dynamics, the method of obtaining smooth continuous trajectories of multiple machines is not sufficient, especially in the coordinated planning of drones, it is difficult to achieve mutual coordination and trajectory between drones and drones. Smoothness requirements.
Using a collaborative trajectory optimization method based on undirected graph formation metrics, the flight information data of the drone is obtained, and the trajectory is drawn using a tri-uniform B-spline curve, and dynamics, trajectory smoothness, formation metrics and based on the characteristics of the B-spline curve are established. The optimization model of safe distance constraints is solved using the L-BFGS algorithm to optimize the coordinated trajectory of multiple drones.
The multi-machine trajectory adjustment is realized under the constraints of the starting and end point position, ensuring that the drone maintains the expected formation structure during formation flight, while meeting the dynamics, trajectory smoothness and safe distance constraints, realizing the multi-machine trajectory adjustment based on the formation requirements. .
Smart Images

Figure CN119002553A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of collaborative trajectory optimization application, and in particular to a collaborative trajectory optimization method and system based on undirected graph array measurement. Background Art
[0002] In recent years, with the improvement of computing power, the trajectory planning problem of determining the starting and ending points is often transformed into an optimization problem for research. Through trajectory optimization, by simply designing a reasonable optimization model, a smooth and continuous trajectory can be obtained while satisfying multiple constraints, and it has a strong trajectory adjustment capability. The collaborative trajectory optimization method plays an important role in multiple application scenarios such as collaborative detection and positioning, collaborative attack, and collaborative search, and is of great significance in improving the system's collaboration, adaptability, and intelligence.
[0003] In the study of multi-machine collaborative planning, it is necessary to consider the mutual coordination between drones, formation requirements and collision detection, and also to consider the requirements of dynamics and trajectory smoothness, which further increases the difficulty of solving. At present, the research on methods to obtain smooth and continuous trajectories of multiple drones under the conditions of formation, safety, dynamics and other constraints is not sufficient. Summary of the invention
[0004] The purpose of the present invention is to provide a collaborative trajectory optimization method and system based on undirected graph formation measurement, which can adjust the trajectories of multiple machines based on formation requirements on the basis of starting and ending position constraints.
[0005] To achieve the above object, the present invention provides the following solutions: In a first aspect, the present invention provides a collaborative trajectory optimization method based on undirected graph array measurement, comprising: Obtain the flight information data of the drone and set constraints; the flight information data includes speed, acceleration, starting point coordinates and end point coordinates.
[0006] According to the flight information data, a cubic uniform B-spline curve is used to draw the trajectories of multiple UAVs.
[0007] According to the multi-UAV trajectories, a collaborative trajectory optimization model is established based on the characteristics of B-spline curves; the collaborative trajectory optimization model is a trajectory optimization model suitable for multi-aircraft formations based on dynamic constraints, trajectory smoothness constraints, formation metric constraints and safety distance constraints within the formation.
[0008] Based on the set constraints, the L-BFGS algorithm is used to solve the collaborative trajectory optimization model to obtain the optimized multi-UAV collaborative trajectory.
[0009] In a second aspect, the present invention provides a collaborative trajectory optimization system based on undirected graph array measurement, comprising: The information acquisition module is used to obtain the flight information data of the UAV and set constraint conditions; the flight information data includes speed, acceleration, starting point coordinates and end point coordinates.
[0010] The trajectory drawing module is used to draw the trajectories of multiple UAVs using a cubic uniform B-spline curve according to the flight information data.
[0011] The model building module is used to establish a collaborative trajectory optimization model according to the multi-UAV trajectories and based on the characteristics of the B-spline curve; the collaborative trajectory optimization model is a trajectory optimization model suitable for a multi-aircraft formation based on dynamic constraints, trajectory smoothness constraints, formation metric constraints and safety distance constraints within the formation.
[0012] The optimization module is used to solve the collaborative trajectory optimization model based on the set constraints and adopt the L-BFGS algorithm to obtain the optimized multi-UAV collaborative trajectory.
[0013] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects: The present invention provides a collaborative trajectory optimization method and system based on undirected graph formation metric, the method comprising: obtaining flight information data of unmanned aerial vehicles and setting constraints; the flight information data comprises speed, acceleration, starting point coordinates and end point coordinates; according to the flight information data, using a cubic uniform B-spline curve to draw multiple unmanned aerial vehicle trajectories; according to the multiple unmanned aerial vehicle trajectories, based on the characteristics of the B-spline curve, establishing a collaborative trajectory optimization model; the collaborative trajectory optimization model is a trajectory optimization model suitable for multiple aircraft formations established based on dynamic constraints, trajectory smoothness constraints, formation metric constraints and safety distance constraints within the formation; based on the set constraints, using the L-BFGS algorithm to solve the collaborative trajectory optimization model, and obtain the optimized collaborative trajectory of multiple unmanned aerial vehicles. The present invention adopts undirected graph formation metric to collaboratively optimize the trajectories of multiple unmanned aerial vehicles, ensuring that the unmanned aerial vehicles can maintain the expected formation structure when flying in formation, while satisfying the constraints of dynamics, trajectory smoothness, formation metric and safety distance within the formation, and realizing the adjustment of the trajectories of multiple aircraft based on formation requirements. BRIEF DESCRIPTION OF THE DRAWINGS
[0014] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0015] Figure 1 A schematic flow chart of a collaborative trajectory optimization method based on undirected graph array measurement provided in Example 1 of the present invention.
[0016] Figure 2 This is a flow chart of L-BFGS provided in Example 1 of the present invention.
[0017] Figure 3 This is a simulation result diagram of Experiment 1 without safety distance constraints provided in Example 1 of the present invention.
[0018] Figure 4 This is a simulation result diagram of Experiment 1 for setting a safety distance constraint provided in Example 1 of the present invention.
[0019] FIG. 5 ( a ) is a schematic diagram of a triangle formation in Experiment 2 provided in Embodiment 1 of the present invention; FIG. 5 ( b ) is a schematic diagram of a relative position relationship in Experiment 2 provided in Embodiment 1 of the present invention.
[0020] FIG6 (a) is a schematic diagram of simulation results of Experiment 2 without formation constraints provided in Example 1 of the present invention; FIG6 (b) is a schematic diagram of simulation results of Experiment 2 with formation constraints provided in Example 1 of the present invention.
[0021] FIG. 7 ( a ) is a schematic diagram of an oblique single-row formation of Experiment 3 provided in Embodiment 1 of the present invention; FIG. 7 ( b ) is a schematic diagram of a relative position relationship of Experiment 3 provided in Embodiment 1 of the present invention.
[0022] FIG8( a ) is a schematic diagram of simulation results of Experiment 3 without formation constraints provided in Example 1 of the present invention; FIG8( b ) is a schematic diagram of simulation results of Experiment 3 with formation constraints provided in Example 1 of the present invention. DETAILED DESCRIPTION
[0023] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0024] The purpose of the present invention is to provide a collaborative trajectory optimization method and system based on undirected graph formation measurement, which can adjust the trajectories of multiple machines based on formation requirements on the basis of starting and ending position constraints.
[0025] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.
[0026] Embodiment 1 like Figure 1As shown, this embodiment provides a collaborative trajectory optimization method based on undirected graph array measurement, including: Step 101: Obtain flight information data of the UAV and set constraints; the flight information data includes speed, acceleration, starting point coordinates and end point coordinates.
[0027] Step 102: Draw multiple UAV trajectories using a cubic uniform B-spline curve based on the flight information data.
[0028] Step 103: According to the multi-UAV trajectories, a collaborative trajectory optimization model is established based on the characteristics of the B-spline curve; the collaborative trajectory optimization model is a trajectory optimization model suitable for a multi-UAV formation that is established based on dynamic constraints, trajectory smoothness constraints, formation metric constraints, and safety distance constraints within the formation.
[0029] Step 104: Based on the set constraints, the collaborative trajectory optimization model is solved using the L-BFGS algorithm to obtain an optimized multi-UAV collaborative trajectory.
[0030] In some implementations of this embodiment, when executing step 102, the specific steps may be as follows: According to the speed, acceleration, starting point coordinates and end point coordinates of the UAV, a cubic uniform B-spline curve is used to describe the trajectory of the UAV.
[0031] Specifically, the B-spline curve is a generalized form of the Bezier curve, which is widely used in the field of aircraft trajectory planning. The degree of the B-spline polynomial can be independent of the number of control points. At the same time, the B-spline curve allows local control and adjustment of the curve. Therefore, when describing the multi-aircraft trajectory form, the uniform cubic B-spline curve is used as the basic expression, as shown below: (1).
[0032] in, is the control point, is the number of control points, basis function It is defined using a recursive formula: (2).
[0033] (3).
[0034] Above The curve control parameters represented are not real time. For the i + k The time parameters of the control points, For the i + k+ Time parameter of 1 control point, subscript , is the order of the spline curve. The time interval between the control points of the uniform B-spline curve Similarly, in order to construct the relationship between real time and curve parameters, a time normalization operation is performed: (4).
[0035] in, Respectively i control points and i +1 time parameter of the control point. Afterwards, the real time between two control points can be converted into curve parameters, that is, the real time is introduced into the B-spline curve design. Construct the time-dependent B-spline curve expression and transform equation (1) into a matrix form: (5).
[0036] (6).
[0037] (7).
[0038] in, , is the order of the spline curve, Substituting into the above formula, we get the matrix , the above matrix expression can be used to obtain the position information of any real time on the B-spline curve.
[0039] (8).
[0040] In some implementations of this embodiment, when executing step 103, the specific steps may be as follows: A collaborative trajectory optimization model is established, and the cost function and gradient representation are constructed based on the characteristics of B-spline curves.
[0041] Specifically, based on the expected formation requirements, a collaborative trajectory optimization model suitable for multi-aircraft formations is established by comprehensively considering constraints such as dynamics, trajectory smoothness, and formation safety, as shown in formula (9): (9).
[0042] in, is a dynamic feasible term to satisfy the dynamic constraints, is the trajectory smoothing term, which ensures the trajectory is smooth and continuous. is the formation measurement item, It is the internal safety item of the formation. , are the weight coefficients of formation measurement item, dynamic feasibility item, trajectory smoothness item and internal safety item of formation.
[0043] (1) Dynamically feasible terms .
[0044] Among them, the derivative of the B-spline curve is still the B-spline curve. The B-spline represents the position information, so the velocity and acceleration information can be obtained by derivation. The basis function form is derived as follows: (10).
[0045] (11).
[0046] in, It is still a B-spline curve, where the control point information is the derivative of the position, that is, the corresponding speed control information, so equation (11) can be expressed as: (12).
[0047] (13).
[0048] Similarly, yes The derivation is as follows: (14).
[0049] in, It is still a B-spline curve, where the control point information is the derivative of the velocity, that is, the corresponding acceleration control information. Therefore, equation (14) can be expressed as equation (15), as follows: (15).
[0050] (16).
[0051] in, is the basis function of B-spline, so the velocity and acceleration information can be calculated as follows, Represents the derivative form of the B-spline curve: (17).
[0052] Obviously, by combining the real-time information, the velocity and acceleration of each position in the trajectory can be calculated. However, in the process of constructing the cost function, the dynamic constraint judgment is not performed on each point in the trajectory, because the control point depicts the direction of the curve. According to formula (17), the "control points" of velocity and acceleration are also used to describe the changing trend of velocity and acceleration. Between two adjacent control points, the velocity and acceleration on the trajectory are within the range of the velocity and acceleration of these two control points. Therefore, it is only necessary to construct the cost function based on formula (17) and the number of control points N, as follows: (18).
[0053] (19).
[0054] in, and For the i The speed and acceleration of the drone at each control point, J d is the dynamic feasible term, is the number of control points, is the maximum speed, is the maximum acceleration.
[0055] (2) Trajectory smoothness term: In the process of adjusting the curve control points, the present embodiment actually performs fine control on the shape of the curve trajectory. This control method is direct and efficient, so that the present embodiment can accurately adjust the curve according to actual needs. It is worth noting that the B-spline curve has a characteristic that its derivative is still a B-spline curve. This characteristic provides great convenience for the present embodiment to control the curve trajectory.
[0056] In this embodiment, the continuity and smoothness of the entire curve trajectory are ensured by reasonably adjusting the change range of the two control points of acceleration and speed. These two control points are like the "engine" of the curve, and their change range directly affects the motion state of the curve. The adjustment of the acceleration control point can change the acceleration of the curve, while the adjustment of the speed control point affects the speed of the curve.
[0057] When adjusting the curve, the following points should be noted: 1. Make sure the adjustment range of the acceleration and speed control points is consistent to ensure the continuity and smoothness of the curve. If there is a large difference between the two, the curve may appear abrupt or jittery at certain positions, affecting the trajectory quality.
[0058] 2. Adjust the control point according to actual needs. For example, in places where rapid acceleration or deceleration is required, this embodiment can appropriately increase the adjustment range of the acceleration or speed control point. This can make the curve better adapt to various complex environments and improve its motion performance.
[0059] 3. Curve control is not limited to B-spline curves. In fact, all kinds of curves, such as Bezier curves, polynomial curves, etc., can achieve shape control by adjusting control points. In this embodiment, the appropriate curve type can be selected according to the specific application scenario.
[0060] 4. When adjusting the control points, the interaction between the curve and other objects or the environment must also be considered. For example, in robot motion planning, this embodiment needs to ensure that the curve does not collide with obstacles to ensure that the robot can successfully complete the task.
[0061] In short, by adjusting the curve control points, this embodiment can achieve direct control of the curve trajectory shape. Reasonable adjustment of the change range of the acceleration and speed control points can ensure the continuity and smoothness of the entire trajectory, thereby improving the trajectory quality. In practical applications, this embodiment should flexibly adjust the control points according to specific needs and scenarios.
[0062] Specifically, the trajectory smoothing term may be as follows: (20).
[0063] (twenty one).
[0064] in, Represent the degree of change of control point, velocity and acceleration respectively. are the weight coefficients of the control point change degree, velocity change degree, and acceleration change degree respectively; For the i control points, For the i -1 control point, For the i +1 control point, is the acceleration, For speed.
[0065] (3) Formation measurement items: In trajectory optimization, in order to measure the gap between the current aircraft formation and the expected formation and design an adjustment strategy, a metric mapping between the actual formation and the expected formation is used to quantitatively describe the formation adjustment effect in three-dimensional space. The formation of multiple aircraft is modeled using a complete undirected graph. In this complete undirected graph, the edge between each two nodes can be assigned a weight coefficient using the Euclidean distance: (twenty two).
[0066] Based on the multi-machine graph model, the adjoint matrix of each motion node can be constructed With degree matrix , where degree refers to the number of edges connecting a node to other nodes. The diagonal elements of the degree matrix represent the degree of each node, while the non-diagonal elements are zero. Normalizing the Laplacian matrix yields the normalized symmetric Laplacian matrix : (twenty three).
[0067] Based on graph theory, the Laplace matrix contains the graph structure information of the formation. In order to describe the degree of conformity between the actual formation and the expected formation, a formation similarity measurement function can be designed.
[0068] (twenty four).
[0069] in, represents the Laplace matrix of the current formation, represents the Laplace matrix corresponding to the desired formation. Since the corresponding graph structures are weighted according to the absolute distance between drones, the metric function is invariant to the rotation and translation of the formation and can be applied to three-dimensional scenes. Based on formula (24), the formation metric cost function is constructed: (25).
[0070] in, is the number of sampling points.
[0071] Since the present invention constructs multi-machine trajectories based on B-spline curves, it is difficult to obtain the entire trajectory information after determining the control point position in one optimization iteration. Therefore, a sampling method is adopted to select the multi-machine positions at several moments on the multi-machine trajectory, and the formation metric function value is calculated using formula (25), and the metric values at each sampling moment are summed to obtain the cost function value.
[0072] To optimize the solution, it is necessary to derive the gradient of the cost function with respect to the optimization variable. Since the metric function in equation (25) has a continuous differentiable characteristic with respect to the aircraft position, the gradient of the metric function with respect to the optimization variable can be calculated to solve the optimization problem. The following is the gradient derivation process: (26).
[0073] in, represents the metric cost function, is the position information of the curve control point, is the trajectory path point, and by the chain rule, we can get: (27).
[0074] Separate the terms in equation (27) to derive the gradient, is the weight vector, Taking the derivative of the elements in the weight vector gives: (28).
[0075] (29).
[0076] (30).
[0077] (31).
[0078] (32).
[0079] Combining equation (29) - equation (32), we can get: (33).
[0080] Formula (33) obtains the partial derivative of the metric function with respect to the trajectory path point. According to the chain rule, we need to calculate , the derivative of the control point is a combination of multiple basis functions, which is difficult to calculate directly, so it is expressed in the form of De Boor algorithm: (34).
[0081] (35).
[0082] in, For the The three-dimensional coordinate information of the control points, To calculate the recursive depth, the control points are directly recursively represented by the De Boor algorithm without calculating After the recursion is completed, the actual trajectory point position is also directly calculated by formula (35). According to the recursive relationship, the following formula can be obtained: (36).
[0083] Among them, after the recursion is completed Corresponding path point information , Refers to any four consecutive control points in the entire trajectory, because the curve shape between two control points of the B-spline curve is only determined by the four adjacent control points, that is, it has the characteristic of local adjustment. Therefore, It can be expressed as: (37).
[0084] Among them, it can be seen from formula (36) that For one The diagonal matrix of : (38).
[0085] in, represents the sampling point in equation (35), and the matrix elements in the above equation are expressed as follows: (39).
[0086] (40).
[0087] In summary, by combining equations (27), (33), (37)-(40), we can obtain the partial derivatives of the formation metric function with respect to the optimized control variables, thus realizing the gradient solution process.
[0088] (4) Safety distance within the formation: When UAVs are flying in formation, they need to maintain a safe distance to prevent collisions. Therefore, a safe distance cost is set: (41).
[0089] in, is the number of sampling points, and Sampling and judgment are performed at each moment. is the number of drones in the formation, is the safety distance. The cost function is constructed only when the distance between drones does not meet the safety constraint. If the safety distance requirement is met, The gradient of equation (41) can be expressed as: (42).
[0090] Therefore, combining equations (37) and (41) (42), the gradient of the safety distance cost function can be expressed as: (43).
[0091] In some implementations of this embodiment, when executing step 104, the specific steps may be as follows: LBFGS is used to solve the optimization problem.
[0092] Specifically, when solving optimization problems, it is necessary to select a reasonable solution algorithm to ensure the calculation accuracy while achieving the highest calculation efficiency. In the optimization model constructed by the present invention, the cost function can obtain a detailed function expression and can be used to derive the control variables. Therefore, the L-BFGS (Limited-memory BFGS) algorithm is used to solve the optimization model. The L-BFGS algorithm has excellent performance. It has the advantages of fast convergence speed and can effectively reduce memory overhead. It is suitable for solving large-scale multi-parameter unconstrained nonlinear optimization problems. The algorithm flow is as follows: Figure 2 As shown: (1) Initialize optimization variables , , , .
[0093] (2) Calculate the direction of gradient descent .
[0094] (3) Use line search method to obtain the optimal step length , construct the sufficient descent condition and the curvature condition: (44).
[0095] (4) Update variables .
[0096] (5) Update gradient .
[0097] (6) Determine the relationship between k and n, if Then the BFGS algorithm in formula (45) is used, and the most recently updated For Elements , and continuously update according to formula (46), otherwise let .
[0098] (45).
[0099] (46).
[0100] in, are the optimization variables and the corresponding gradients, Represents the current Iteration steps, n means only save n iteration information.
[0101] (7) Set the algorithm convergence conditions: (47).
[0102] is a very small positive number. Specifically, in optimization problems, the goal is to find a set of variables (control variables) so that a cost function reaches the minimum (or maximum) value. The L-BFGS (Limited-memory BFGS) algorithm is a commonly used quasi-Newton method for solving unconstrained nonlinear optimization problems, especially for large-scale high-dimensional problems.
[0103] Therefore, in the process of solving the problem, first of all, it is necessary to set the initial value of the optimization variable, as well as some algorithm parameters, such as the learning rate (step size) α, and the matrix used to store historical information; then, calculate the gradient of the cost function with respect to the variable in the current state, that is, the partial derivative of the function, which indicates the direction in which the function value decreases fastest; through a one-dimensional line search method (such as the Armijo rule), find a suitable step size α in the direction of gradient descent, so that descending in this direction can not only fully reduce the function value, but also meet certain curvature conditions to ensure convergence; according to the optimal step size found, update the value of the variable, that is, move to the new position; calculate the gradient at the new position; L-BFG The S algorithm uses a limited memory strategy to approximate the Hessian matrix (second-order derivative matrix) and only stores the latest n update information; updates this approximate matrix according to the BFGS formula for subsequent variable updates; checks whether the current optimization state meets the preset convergence conditions, such as the norm of the gradient is less than a threshold, or the function value changes very little over several consecutive iterations; if so, the algorithm ends and the current variable value is the optimization result; otherwise, returns to step 2 and continues the next round of iterations; while ensuring convergence, the L-BFGS algorithm reduces the complexity of storage and calculation due to its limited memory characteristics, so it has high efficiency when dealing with large-scale optimization problems.
[0104] In this embodiment, some simulation examples are also provided, as follows: In order to verify the collaborative adjustment capability of the constructed collaborative trajectory optimization method for multiple trajectories and analyze its formation construction effect, this embodiment constructs a multi-UAV flight scene for simulation experiments. Considering the single-machine dynamics constraints, the maximum speed , maximum acceleration In order to better display the trajectory adjustment effect, the flight altitude is set unchanged. The specific simulation scene settings are as follows: (1) Experiment 1: To verify that the constructed safety distance cost function can play a role in the optimization process.
[0105] (2) Experiment 2: In order to verify that the constructed collaborative trajectory optimization model has the ability to adjust the trajectory, a triangular formation was set up for simulation experiments.
[0106] (3) Experiment 3: Based on Experiment 2, the formation constraints were changed and the formation requirement was set to an oblique single-row formation.
[0107] Specifically, the experimental structure of Experiment 1 is as follows: In Experiment 1, an optimization item describing the safety distance within the formation was constructed, so a simulation experiment will be conducted on this optimization item to verify whether it can adjust the trajectory to meet the safety distance requirements within the formation. The initial condition settings are shown in Table 1, and a safety distance constraint of 120m is set.
[0108] Table 1 Initial conditions of Experiment 1
[0109] The simulation results are as follows Figure 3 and Figure 4 As shown in the figure, the trajectory status of the four UAVs is displayed, and their trajectory points at multiple moments are connected to each other to more clearly show the relative position relationship between the multiple UAVs at the same moment. When there is no safety distance constraint, the four UAVs maintain a straight trajectory flight. After setting the safety distance constraint, the trajectory optimization adjusts the curve control points to obtain a trajectory that meets the safety distance constraint. However, due to the constraints of the start and end positions, the optimization method only adjusts the trajectory of the intermediate process. The experimental results show that the established safety distance term can effectively control the trajectory of multiple UAVs, thereby meeting the safety distance constraint.
[0110] Specifically, the experimental results of Experiment 2 are as follows: In order to verify whether the constructed collaborative trajectory optimization model has the ability to adjust the trajectory form to meet the corresponding formation constraints, a three-machine collaborative scenario is established, and the safety distance constraint is set to 120m. The specific parameters are shown in Table 2: Table 2 Initial conditions of Experiment 2
[0111] The triangular formation constraint is set as shown in Figure 5 (a), and the relative position relationship is shown in Figure 5 (b). The expected formation is an obtuse triangle. The simulation results are shown in Figures 6 (a) and 6 (b). The trajectory points of the drones at multiple times are connected to each other to more clearly show the relative position relationship between multiple drones at the same time. When there is no formation constraint, the three drones have the same flight state and their relative positions remain in a straight formation. After the formation constraint is set, the three trajectories form a triangular formation during the flight through trajectory optimization design, and at the end of the trajectory, they smoothly return to the straight formation state according to the terminal position constraint. However, due to the existence of the initial and final position constraints, the optimization method has limited adjustment space for the trajectory and cannot be completely consistent with the expected formation.
[0112] Specifically, the experimental results of Experiment 3 are as follows: In order to make the simulation experiment more rigorous, a four-machine collaborative scenario was set to verify the ability of the constructed optimization method to adjust the trajectory. The safety distance constraint was set to 120m. The simulation parameters are shown in Table 3: Table 3 Initial conditions of Experiment 3
[0113] The formation constraints shown in Figure 7 (a) are set, and the relative position relationship is shown in Figure 7 (b). The expected formation is an oblique single-row formation. The simulation results are shown in Figure 8 (a). When there is no formation constraint, the four drones have the same flight state and the relative positions remain in a straight formation; after the formation constraint is set, the four trajectories form an oblique single-row formation during the flight. In addition, in Figure 8 (b), at the same time, trajectory 4 arrives at the terminal position faster than trajectory 1. This is because under the initial and final position constraints, in order to meet the formation requirements and dynamic constraints, there will be differences in the trajectory length and arrival time between the drones. Through the above simulation experiments, it can be verified that the optimization method constructed by the present invention has the ability to adjust the curved trajectory to form a corresponding formation.
[0114] Embodiment 2 This embodiment provides a collaborative trajectory optimization system based on undirected graph formation measurement, including: The information acquisition module is used to obtain the flight information data of the UAV and set constraint conditions; the flight information data includes speed, acceleration, starting point coordinates and end point coordinates.
[0115] The trajectory drawing module is used to draw the trajectories of multiple UAVs using a cubic uniform B-spline curve according to the flight information data.
[0116] The model building module is used to establish a collaborative trajectory optimization model according to the multi-UAV trajectories and based on the characteristics of the B-spline curve; the collaborative trajectory optimization model is a trajectory optimization model suitable for a multi-aircraft formation based on dynamic constraints, trajectory smoothness constraints, formation metric constraints and safety distance constraints within the formation.
[0117] The optimization module is used to solve the collaborative trajectory optimization model based on the set constraints and adopt the L-BFGS algorithm to obtain the optimized multi-UAV collaborative trajectory.
[0118] In summary, the present invention has the following technical effects: The present invention proposes a collaborative trajectory optimization method based on undirected graph formation measurement. The method adopts an undirected graph to model the relative position relationship of UAVs in the formation, establishes a formation measurement cost function based on the Laplace matrix, and realizes the adjustment of the trajectories of multiple UAVs to meet the formation constraint requirements under multiple constraints, so as to obtain a continuous trajectory that meets the dynamics and smoothness requirements: (1) The undirected graph is used to model the relative position relationship of UAVs in the formation, which more accurately describes the formation constraints; (2) A multi-objective optimization model is established, which comprehensively considers the optimization objectives such as formation requirements, dynamic constraints, and safe distance within the formation, so as to obtain a more reasonable optimization result that conforms to the characteristics of collaborative motion; (3) The partial derivatives of each cost function with respect to the optimization variable are derived to perform gradient optimization solution, which has good optimization efficiency; (4) The collaborative trajectory optimization model established by the present invention has good scene adaptability. It only needs to change or adjust the cost function to meet different scene and task requirements, and obtain smooth and continuous trajectories of multiple UAVs that meet the requirements.
[0119] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0120] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of the present invention. At the same time, for those skilled in the art, according to the idea of the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.
Claims
1. A collaborative trajectory optimization method based on undirected graph array measurement, characterized in that: include: Obtaining flight information data of the drone and setting constraints; the flight information data includes speed, acceleration, starting point coordinates and end point coordinates; Based on the flight information data, a cubic uniform B-spline curve is used to draw the trajectories of multiple UAVs; According to the multi-UAV trajectories, a collaborative trajectory optimization model is established based on the characteristics of the B-spline curve; the collaborative trajectory optimization model is a trajectory optimization model suitable for a multi-UAV formation established based on dynamic constraints, trajectory smoothness constraints, formation metric constraints, and safety distance constraints within the formation; Based on the set constraints, the L-BFGS algorithm is used to solve the collaborative trajectory optimization model to obtain the optimized multi-UAV collaborative trajectory.
2. The collaborative trajectory optimization method based on undirected graph array measurement according to claim 1 is characterized in that: The trajectory expression of the multi-UAV trajectory is as follows: ; ; in, For the control points, is the basis function; For the The time parameter of the control point, subscript , is the order of the spline curve, is the B-spline curve trajectory, is the number of control points, t is the curve control parameter, For the i + k The time parameters of the control points, For the i + k +1 time parameter for the control point, and are basis functions under different control points.
3. The collaborative trajectory optimization method based on undirected graph array measurement according to claim 1 is characterized in that: The set constraint condition is one or more of a safety distance constraint between drones or a drone formation constraint.
4. The collaborative trajectory optimization method based on undirected graph array measurement according to claim 1 is characterized in that: The dynamic constraint is determined according to the dynamic feasible term; the formula expression of the dynamic feasible term is: ; in, , , and For the i The speed and acceleration of the drone at each control point, J d is the dynamic feasible term, is the number of control points, is the maximum speed, is the maximum acceleration.
5. The collaborative trajectory optimization method based on undirected graph array measurement according to claim 1 is characterized in that: The trajectory smoothness constraint is determined according to the trajectory smoothness term; the formula expression of the trajectory smoothness term is: ; in, are the degree of change of control point, velocity and acceleration respectively, , are the weight coefficients of the control point change degree, velocity change degree and acceleration change degree, respectively. J s is the trajectory smoothing term.
6. The collaborative trajectory optimization method based on undirected graph array measurement according to claim 1 is characterized in that: The formation metric constraint is determined according to the formation metric item; the formula expression of the formation metric item is: ; in, is the number of sampling points; represents the Laplace matrix of the current formation, represents the Laplace matrix corresponding to the desired formation, J f It is a formation measurement item.
7. The collaborative trajectory optimization method based on undirected graph array measurement according to claim 1 is characterized in that: The safety distance constraint within the formation is determined according to the safety item within the formation; the formula expression of the safety item within the formation is: ; in, is the number of sampling points; is the number of control points; For a safe distance, For the The current position of the UAV, P j+1 For the +1 The current position of the drone. J Safe It is an internal security item of the formation.
8. The collaborative trajectory optimization method based on undirected graph array measurement according to claim 1 is characterized in that: Based on the constraints, the L-BFGS algorithm is used to solve the collaborative trajectory optimization model to obtain the optimized multi-UAV collaborative trajectory, which specifically includes: Set the initial values of the optimization variables in the L-BFGS algorithm; Based on the optimization variables, the gradients of the dynamic constraint, the trajectory smoothness constraint, the formation metric constraint, and the safety distance constraint within the formation are calculated in the current state, and the optimal gradient direction is determined; Based on the optimal gradient direction, the optimal step size is determined using a line search method: According to the optimal step length, updating the optimization variable to obtain an updated optimization variable; According to the updated optimization variable, the gradient at the updated position is calculated, and the BFGS matrix is updated to obtain an updated BFGS matrix; Based on the updated BFGS matrix, determine whether the current optimization state meets the preset convergence conditions; If yes, then the optimized multi-UAV collaborative trajectory is obtained; If not, return to step: based on the optimization variables, calculate the gradients of the dynamic constraints, the trajectory smoothness constraints, the formation metric constraints, and the safety distance constraints within the formation in the current state, and determine the optimal gradient direction until the preset convergence conditions are met.
9. A collaborative trajectory optimization system based on undirected graph array measurement, characterized in that: include: An information acquisition module is used to acquire the flight information data of the UAV and set constraint conditions; the flight information data includes speed, acceleration, starting point coordinates and end point coordinates; A trajectory drawing module, used to draw multiple UAV trajectories using a cubic uniform B-spline curve according to the flight information data; A model building module is used to establish a collaborative trajectory optimization model based on the B-spline curve characteristics according to the multi-UAV trajectories; the collaborative trajectory optimization model is a trajectory optimization model suitable for a multi-UAV formation based on dynamic constraints, trajectory smoothness constraints, formation metric constraints, and safety distance constraints within the formation; The optimization module is used to solve the collaborative trajectory optimization model based on the set constraints and adopt the L-BFGS algorithm to obtain the optimized multi-UAV collaborative trajectory.
Citation Information
Cited By
Low-altitude aircraft air traffic management method and system
CN119905019A
Completely distributed unmanned aerial vehicle cluster communication method and system, medium and product
CN120406566A
Aircraft large-scale trajectory planning method based on feature space collocation
CN120871980A
Unmanned aerial vehicle cluster formation control method and system based on multi-level intelligent algorithm
CN121635470A
