UAV-UAV joint trajectory optimization method, system, medium and equipment

Abstract

The present invention belongs to the field of trajectory optimization, and provides a UAV-unmanned vehicle joint trajectory optimization method, system, medium and equipment. The technical solution is to integrate the trajectory characteristics of the UAV and the unmanned vehicle, integrate the motion characteristics, environmental constraints and task requirements of the UAV and the unmanned vehicle for comprehensive consideration, and complete the collaborative tasks of the UAV and the unmanned vehicle with high efficiency and high quality, ensuring that the UAV and the unmanned vehicle fully tap their collaborative capabilities. Unconstrained trajectory optimization is adopted, and all constraint penalty items are set as soft constraint items. Specific requirements are made on the trajectory according to a specific task, such as requiring the trajectory to pass through a specific point, and it is also convenient to add new constraint penalty items to the trajectory; energy consumption constraint penalty items are added to coordinate with time regularization items to reduce the energy consumption of the UAV; landing terminal constraints are imposed on the trajectory with landing tasks, so that the UAV can obtain a safe and efficient landing quality at the convergence point.

Description

Technical Field

[0001] The present invention belongs to the field of trajectory optimization, and in particular relates to a method and system for unmanned aerial vehicle (UAV)-unmanned vehicle (UAV) joint trajectory optimization. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] Multi-rotor drones are widely used in logistics, military surveys, circuit inspections, disaster relief and many other fields due to their small size, flexible operation and large range of action. However, drones have limited endurance and load-bearing capacity, and need to collaborate with other intelligent entities, especially unmanned vehicles, to complete large and complex tasks with the advantages of large carrying capacity and long endurance of unmanned vehicles. In the entire unmanned system, drones and unmanned vehicles perform their respective tasks and cooperate with each other when necessary. For example, in a logistics distribution scenario, an unmanned vehicle carrying a large amount of goods travels along a planned route, and the drone delivers the goods on the unmanned vehicle to the designated area at any time and returns to the unmanned vehicle autonomously. In this task, both the drone and the unmanned vehicle need to perform trajectory planning and generate their own collision-free trajectories based on the mission requirements and environmental information.

[0004] In existing trajectory optimization methods, the trajectories of unmanned vehicles and drones are often optimized independently, which makes the unmanned system weak in completing complex tasks and cannot ensure the trajectory quality at the convergence point. In addition, the trajectory of the unmanned vehicle is two-dimensional while the trajectory of the drone is three-dimensional. This mismatch in spatial dimensions also poses challenges to joint trajectory optimization. Summary of the invention

[0005] In order to solve at least one technical problem existing in the above-mentioned background technology, the present invention provides a UAV-Unmanned Vehicle joint trajectory optimization method and system, which, after generating discrete path points and a safe feasible domain, the UAV and the Unmanned Vehicle generate a reference trajectory according to the safe feasible domain and the starting point, and divide the trajectory into multiple path points and corresponding times according to the expected speed; construct a joint trajectory optimization function of the UAV and the Unmanned Vehicle, and then add obstacle avoidance constraint penalty items, instantaneous state constraint penalty items, height constraint penalty items, energy consumption constraint penalty items, time regularization items and terminal constraint penalty items, and finally generate a trajectory that meets the task requirements.

[0006] In order to achieve the above object, the present invention adopts the following technical solution:

[0007] A first aspect of the present invention provides a UAV-UAV joint trajectory optimization method, comprising the following steps:

[0008] Generate discrete path points and safe feasible domains for UAVs and unmanned vehicles based on mission requirements and environmental information;

[0009] Generate reference trajectories according to the starting positions and safe feasible domains of the UAV and the UGV respectively, and represent the reference trajectories as multiple path points and the time corresponding to each path point according to the expected speed;

[0010] Construct a joint trajectory optimization function for drones and unmanned vehicles;

[0011] Construct constraint functions for drones and unmanned vehicles, including: constructing obstacle avoidance constraint penalty items based on the safe feasible domains of drones and unmanned vehicles, constructing instantaneous state constraint penalty items based on the dynamic models of drones and unmanned vehicles, imposing height constraints on the position of unmanned vehicles, and constructing energy consumption constraint penalty items based on the drone energy consumption model; calculating the gradient of each constraint with respect to the constraint variable;

[0012] The optimized path points and the corresponding time are obtained by solving the joint trajectory optimization function of the UAV and the unmanned vehicle and the constraint function of the UAV and the unmanned vehicle, and the final optimized trajectory is obtained by using the optimized path points and the corresponding time representation.

[0013] Furthermore, the joint trajectory optimization function of the UAV and the unmanned vehicle is:

[0014] ,

[0015] in, is the flat output of the real-time status of the drone, is the flat output of the real-time status of the unmanned vehicle, is the total task time, is the current time, is a positive diagonal matrix of suitable dimension, is the order of the flat output quantity.

[0016] Furthermore, the gradient of each constraint with respect to the constraint variable is calculated, including:

[0017] The obstacle avoidance constraint penalty item is the gradient of the coordinates, the instantaneous state constraint penalty item of velocity, acceleration, attitude angle and thrust is the gradient of the UAV speed, acceleration and jerk, the instantaneous state constraint penalty item of velocity, longitudinal acceleration, tangential acceleration and curvature is the gradient of the unmanned vehicle speed and acceleration; the height constraint penalty item is the gradient of the unmanned vehicle height; the energy consumption constraint penalty item is the gradient of the UAV speed.

[0018] Furthermore, the coordinates of the drone or unmanned vehicle at a certain moment or For the safe feasible domain The penalty term for the obstacle avoidance constraint of the hyperplane is set to or :

[0019] ,

[0020] ,

[0021] in, and are the coordinates of the UAV or the unmanned vehicle at a certain moment, and They are the first The normal vector of the hyperplane is and are fixed parameters of the hyperplane.

[0022] Furthermore, when constructing the instantaneous state constraint penalty term, it includes:

[0023] For drones Coordinates of time For example, the speed constraint penalty term is constructed according to the maximum speed constraint, maximum acceleration constraint, maximum pitch angle constraint and maximum thrust constraint. , acceleration constraint penalty , attitude angle constraint penalty and thrust constraint penalty :

[0024] (4),

[0025] (5),

[0026] (6),

[0027] (7),

[0028] in, , and Representing the drones The velocity, acceleration and jerk at each moment, is the attitude angle of the UAV expressed by the UAV speed, acceleration and jerk, is the propeller thrust of the drone expressed in terms of the drone speed, acceleration and jerk, and is the maximum speed, maximum acceleration, maximum attitude angle and maximum thrust of the UAV;

[0029] For unmanned vehicles Coordinates of time For example, the speed constraint penalty term is constructed according to the maximum speed constraint, the maximum longitudinal acceleration constraint, the maximum tangential acceleration constraint and the maximum curvature constraint. , longitudinal acceleration constraint penalty , tangential acceleration constraint penalty and the curvature constraint penalty :

[0030] ,

[0031] ,

[0032] ,

[0033] ,

[0034] in, and Representing the unmanned vehicle The speed and acceleration of the moment, and are the maximum speed, maximum longitudinal acceleration, maximum tangential acceleration and maximum curvature of the unmanned vehicle, is an auxiliary antisymmetric matrix.

[0035] Furthermore, if the mission requirement includes a landing mission, it also includes constructing a terminal constraint penalty item for the UAV, and obtaining the gradient of the terminal constraint penalty item for each attitude quantity of the UAV.

[0036] Furthermore, the step of solving the optimized path points and corresponding times according to the joint trajectory optimization function of the UAV and the unmanned vehicle and the constraint function of the UAV and the unmanned vehicle comprises the following steps:

[0037] Perform gradient transfer on constraint variables, converting the gradient of existing penalty items to each constraint variable into the gradient of constraint items to path points and time;

[0038] Perform spatiotemporal mapping of path points and time to eliminate time and space constraints, and use the mapped path points and time as optimization variables;

[0039] The optimization variables and joint trajectory optimization function are put into the solver for solving to obtain the optimized path points and corresponding time.

[0040] A second aspect of the present invention provides a UAV-UAV joint trajectory optimization system, comprising:

[0041] The initial path generation module is used to generate discrete path points and safe feasible domains for the UAV and the unmanned vehicle according to the mission requirements and environmental information; generate reference trajectories according to the starting positions and safe feasible domains of the UAV and the unmanned vehicle respectively, and represent the reference trajectories as multiple path points and the time corresponding to each path point according to the expected speed;

[0042] Optimization model building module, used to build joint trajectory optimization functions for drones and unmanned vehicles;

[0043] Construct constraint functions for drones and unmanned vehicles, including: constructing obstacle avoidance constraint penalty items based on the safe feasible domains of drones and unmanned vehicles, constructing instantaneous state constraint penalty items based on the dynamic models of drones and unmanned vehicles, imposing height constraints on the position of unmanned vehicles, and constructing energy consumption constraint penalty items based on the drone energy consumption model; calculating the gradient of each constraint with respect to the constraint variable;

[0044] The trajectory optimization module is used to solve the optimized path points and corresponding times according to the joint trajectory optimization function of the UAV and the unmanned vehicle and the constraint function of the UAV and the unmanned vehicle, and use the optimized path points and corresponding time to express the final optimized trajectory.

[0045] A third aspect of the present invention provides a computer-readable storage medium.

[0046] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps in the above-mentioned UAV-UAV joint trajectory optimization method.

[0047] A fourth aspect of the present invention provides a computer device.

[0048] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps in the above-mentioned UAV-UAV joint trajectory optimization method are implemented.

[0049] Compared with the prior art, the present invention has the following beneficial effects:

[0050] 1. The present invention combines the trajectory characteristics of UAVs and unmanned vehicles, integrates the motion characteristics, environmental constraints and mission requirements of UAVs and unmanned vehicles for comprehensive consideration, completes the collaborative tasks of UAVs and unmanned vehicles with high efficiency and high quality, ensures that UAVs and unmanned vehicles fully tap their collaborative capabilities, and provides support for the subsequent completion of more complex tasks.

[0051] 2. The present invention adopts unconstrained trajectory optimization and sets all constraint penalty items as soft constraint items. Specific requirements can be made on the trajectory according to a specific task, such as requiring the trajectory to pass through a specific point, and it is also convenient to add new constraint penalty items to the trajectory; the energy consumption constraint penalty item is added to adjust the coordination with the time regularization item, which reduces the energy consumption of the UAV and improves the flight performance of the UAV while ensuring that the mission time is not too long; the landing terminal constraint is imposed on the trajectory with the landing mission, so that the UAV can obtain a safe and efficient landing quality at the convergence point.

[0052] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0054] Figure 1 It is a schematic diagram of the overall process of the UAV-UAV joint trajectory optimization method provided by an embodiment of the present invention;

[0055] Figure 2 It is a detailed flow chart of the UAV-UAV joint trajectory optimization method provided by an embodiment of the present invention;

[0056] Figure 3 is a top-view result diagram of a discrete path showing obstacles without trajectory optimization provided by an embodiment of the present invention;

[0057] Figure 4 is a side view result diagram of a discrete path without trajectory optimization and without displaying obstacles provided by an embodiment of the present invention;

[0058] Figure 5 is a top-view result diagram showing a smooth trajectory of an obstacle after trajectory optimization provided by an embodiment of the present invention;

[0059] Figure 6 This is a side view result diagram of a smooth trajectory without obstacles after trajectory optimization provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0060] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0061] It should be noted that the following detailed descriptions are all illustrative and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0062] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0063] Embodiment 1

[0064] like Figure 1 and Figure 2 As shown, this embodiment provides a UAV-UAV joint trajectory optimization method, comprising the following steps:

[0065] Step 1: Generate discrete path points and safe feasible domains for UAVs and unmanned vehicles based on mission requirements and environmental information;

[0066] The starting points of the UAV and the unmanned vehicle are set according to the mission requirements, and discrete path points are generated as the initial path according to the graph search algorithm, such as Figure 3 Shown is a top view of the discrete path after path planning; Figure 4 is Figure 3 A side view of a version without visualizing obstacles. The red ball at height represents the drone, the red ball on the ground represents the unmanned vehicle, the yellow ball in the middle represents the meeting point, and the route is represented by a red line.

[0067] According to the environmental information, a fast regional iterative expansion algorithm is used to generate multiple safety corridors surrounded by polyhedrons;

[0068] In this embodiment, the graph search algorithm can be used as follows Algorithms such as choosing a bidirectional The search algorithm generates an initial path for the drone to merge with the unmanned vehicle;

[0069] In this embodiment, the fast region iterative expansion algorithm uses a method of expanding the inscribed ellipsoid of a polyhedron to obtain the largest convex polyhedron as a safe feasible region.

[0070] Step 2: Generate reference trajectories based on the starting positions and safe feasible domains of the UAV and the UGV respectively, and divide the reference trajectories into multiple path points and the time corresponding to each path point according to the expected speed, and use the path points and the corresponding time to represent the reference trajectories;

[0071] Step 3: Construct the joint trajectory optimization function of the UAV and the unmanned vehicle;

[0072] In this embodiment, the path points of the unmanned vehicle are first regarded as three-dimensional coordinates to ensure that the trajectory optimization process of the drone and the unmanned vehicle is consistent in dimension, and the trajectory optimization functions of the unmanned vehicle and the drone are written as a joint trajectory optimization function :

[0073] (1),

[0074] in, is the flat output of the real-time status of the drone, is the flat output of the real-time status of the unmanned vehicle, is the total task time, is the current time, is a positive diagonal matrix of suitable dimension, is the order of the flat output quantity, and its value depends on the specific system dynamics and the setting of the optimization problem. In this embodiment, in order to ensure the smoothness of the trajectory and reduce mechanical stress, .

[0075] Step 4: Construct constraint functions for drones and unmanned vehicles;

[0076] The specific steps include:

[0077] Step 401: construct obstacle avoidance constraint penalty items according to the safe feasible regions of the UAV and the unmanned vehicle, respectively, and obtain the gradients of the items with respect to the positions of the UAV and the unmanned vehicle;

[0078] In this embodiment, according to the generated safe feasible domain, an obstacle avoidance constraint penalty item is constructed for the positions of the UAV and the unmanned vehicle, so that they are always located within the safe feasible domain, realizing the obstacle avoidance function, and obtaining the gradient of the item for the positions of the UAV and the unmanned vehicle.

[0079] The coordinate point of the drone or unmanned vehicle at a certain moment or For the safe feasible domain The penalty term for the obstacle avoidance constraint of the hyperplane is set to or :

[0080] (2),

[0081] (3),

[0082] in, and are the coordinates of the UAV or the unmanned vehicle at a certain moment, and They are the first The normal vector of the hyperplane is and are fixed parameters of the hyperplane.

[0083] The gradient of the constructed obstacle avoidance constraint penalty term with respect to the coordinate value can be obtained and , the construction of obstacle avoidance constraint penalty term is extended to all coordinate points and their corresponding hyperplanes. After adding, the total obstacle avoidance constraint penalty term and the gradient of each term with respect to the coordinate can be obtained.

[0084] Step 402: construct instantaneous state constraint penalty terms according to the dynamic models of the UAV and the unmanned vehicle, and obtain the gradients of multiple physical quantities of the instantaneous state constraint penalty terms for the UAV and the unmanned vehicle;

[0085] In this embodiment, the dynamic constraints of the UAV and the unmanned vehicle are taken into consideration, and their instantaneous state constraint penalty items are constructed according to their respective dynamic models. The instantaneous state constraint penalty items of velocity, acceleration, attitude angle and thrust are constructed for the UAV, and the instantaneous state constraint penalty items of velocity, longitudinal acceleration, tangential acceleration and curvature are constructed for the unmanned vehicle, and the gradients of all instantaneous state constraint penalty items on the coordinates, velocity, acceleration and other physical quantities of the UAV and the unmanned vehicle are obtained.

[0086] For drones Coordinates of time For example, the speed constraint penalty term is constructed according to the maximum speed constraint, maximum acceleration constraint, maximum pitch angle constraint and maximum thrust constraint. , acceleration constraint penalty , attitude angle constraint penalty and thrust constraint penalty :

[0087] (4),

[0088] (5),

[0089] (6),

[0090] (7),

[0091] in, , and Representing the drones The velocity, acceleration and jerk at each moment, is the attitude angle of the UAV expressed by the UAV speed, acceleration and jerk, is the propeller thrust of the drone expressed in terms of the drone speed, acceleration and jerk, and is the maximum speed, maximum acceleration, maximum attitude angle and maximum thrust of the drone. The attitude angle includes pitch angle, roll angle and yaw angle. Due to the consistent format, the present embodiment uniformly uses the attitude angle formula to represent it.

[0092] Obtain the gradient of the above four penalty terms for the UAV speed , , , the gradient of the drone's acceleration , and and the gradient of the UAV's acceleration , .

[0093] For unmanned vehicles Coordinates of time For example, the speed constraint penalty term is constructed according to the maximum speed constraint, the maximum longitudinal acceleration constraint, the maximum tangential acceleration constraint and the maximum curvature constraint. , longitudinal acceleration constraint penalty , tangential acceleration constraint penalty and the curvature constraint penalty :

[0094] (8),

[0095] (9),

[0096] (10),

[0097] (11),

[0098] in, and Representing the unmanned vehicle The speed and acceleration of the moment, and are the maximum speed, maximum longitudinal acceleration, maximum tangential acceleration and maximum curvature of the unmanned vehicle, is an auxiliary antisymmetric matrix.

[0099] Obtain the gradient of the above four penalty terms for the speed of the unmanned vehicle , , , and the gradient of the unmanned vehicle's acceleration , and .

[0100] Step 403: impose a height constraint on the position of the unmanned vehicle so that its range of activity is a two-dimensional plane.

[0101] The coordinates of the unmanned vehicle are regarded as three-dimensional coordinate points. At this time, a constraint penalty is imposed on the height of the unmanned vehicle path points. Altitude coordinates at time Constructing a height constraint penalty :

[0102] (12),

[0103] in For driverless cars Moment coordinate.

[0104] Obtain the gradient of the height constraint penalty term with respect to the height of the unmanned vehicle By adding this item, the coordinates of the unmanned vehicle can be restricted to two-dimensional coordinate points, so that it can avoid the difficulties in the joint process caused by different dimensions while satisfying the real dimensions of the unmanned vehicle. After obtaining the height constraint penalty item of the unmanned vehicle and its gradient with respect to the height coordinate, all the constraint penalty items are added together as the total constraint penalty item.

[0105] Step 404: construct an energy consumption constraint penalty item according to the UAV energy consumption model, and obtain the gradient of the energy consumption constraint penalty item with respect to the UAV speed;

[0106] In this embodiment, a test flight experiment is conducted by setting different speed values ​​to detect the energy consumption of the drone, and the functional relationship between its energy consumption item and the speed of the drone is obtained by fitting. , construct the energy consumption constraint penalty term and the gradient of the term with respect to speed according to the functional relationship .

[0107] At the same time, in order to prevent the situation where the flight speed is too slow and the trajectory time is too long due to the pursuit of too low energy consumption, a time regularization term is added to penalize the trajectory duration to avoid the situation where the mission time is too long.

[0108] Step 405: Add all constraint items from step 401 to step 404 to obtain a total penalty item;

[0109] Step 406: determine whether the mission includes a landing mission. If so, construct a terminal constraint penalty term and obtain the gradient of the terminal constraint penalty term for each attitude value of the drone.

[0110] If the mission includes a landing mission, a terminal constraint penalty item is constructed to ensure the landing quality in the landing scenario. During the trajectory optimization process, the state at the confluence point and the continuity of the overall mission are integrated, and the state of the unmanned vehicle at the confluence point is set as the reference quantity for the drone landing process. The position, speed, and acceleration of the unmanned vehicle at the confluence moment are extracted, and the attitude angle required for the drone to land here is obtained based on the top attitude of the unmanned vehicle and the terrain. Using these quantities as a reference, the position, speed, acceleration, and attitude angle of the drone when it is about to reach the confluence point are constrained, so that the drone can land accurately on the unmanned vehicle, improving the landing quality and the continuity of subsequent tasks. Construct the terminal constraint penalty item and the gradient of the item for the drone position, speed, and acceleration, and finally add the penalty item to the total penalty item.

[0111] Step 5: Add all penalty items to the constraint function as soft constraint items;

[0112] The specific steps include:

[0113] Step 501: perform gradient transfer on all constraint variables, and convert the gradient of the existing penalty item to each constraint variable into the gradient of the penalty item to the path point and time;

[0114] The total penalty term already includes all penalty terms and the gradient of each penalty term for each physical quantity at each moment. According to the gradient propagation chain, the gradients of all penalty terms for the position, speed, acceleration and other physical quantities of the UAV and the unmanned vehicle are all converted into the gradients for the path points of the UAV and the unmanned vehicle and the corresponding time.

[0115] Step 502: Perform time-space mapping on the path points and time to eliminate time and space constraints, and use the mapped path points and time as optimization variables;

[0116] For time constraints, the time at a certain path point is Converted into an auxiliary time variable , to eliminate the original time constraint greater than zero, where and The functional relationship is:

[0117] (13),

[0118] in, is the time corresponding to the path point in the original trajectory, The corresponding auxiliary time variable after mapping.

[0119] For spatial constraints, considering that the original path points are restricted to a closed and convex polyhedral space, the vertices of each polyhedron are calculated using the convex hull algorithm, and any point inside the polyhedron is represented as a convex combination of the polyhedral vertices. Then, the element-by-element square mapping is used to eliminate the non-negativity constraints and transform the original polyhedral constraints into a new closed sphere constraint. Finally, a smooth surjection is applied to map the points in the unit sphere to the geometric space, so that the points in the polyhedron correspond to the points in the entire geometric space, and the mapped points in the geometric space are used as coordinate points in the optimization variables. This process successfully eliminates the influence of the polyhedron boundary on trajectory optimization without directly processing the original geometric constraints, thus achieving the effect of converting the problem originally with geometric restrictions into an unconstrained optimization problem.

[0120] Step 503: After removing the original constraints of time and space, all penalty items are added to the optimization function as soft constraints to achieve unconstrained trajectory optimization.

[0121] Step 504: Put the optimization function and the variables to be optimized into the solver for solving, obtain the optimized path points and the corresponding time, and use the optimized path points and the corresponding time to express the final optimized trajectory;

[0122] Figure 5 is a top view of the smooth trajectory after trajectory optimization. Figure 6 yes Figure 5 The side view of the version without visualizing obstacles based on the results of the present invention, the red ball with height on the left side of the picture represents the UAV, the red ball on the right side on the ground represents the unmanned vehicle, the yellow ball in the middle represents the confluence point, and the smooth trajectory after trajectory optimization is represented by a blue line. It can be seen that the algorithm proposed in the present invention realizes the joint trajectory optimization of the UAV and the unmanned vehicle, and the effect is good.

[0123] Embodiment 2

[0124] This embodiment provides a UAV-UAV joint trajectory optimization system, including:

[0125] The initial path generation module is used to generate discrete path points and safe feasible domains for the UAV and the unmanned vehicle according to the mission requirements and environmental information; generate reference trajectories according to the starting positions and safe feasible domains of the UAV and the unmanned vehicle respectively, and represent the reference trajectories as multiple path points and the time corresponding to each path point according to the expected speed;

[0126] Optimization model building module, used to build joint trajectory optimization functions for drones and unmanned vehicles;

[0127] Construct constraint functions for drones and unmanned vehicles, including: constructing obstacle avoidance constraint penalty items based on the safe feasible domains of drones and unmanned vehicles, constructing instantaneous state constraint penalty items based on the dynamic models of drones and unmanned vehicles, imposing height constraints on the position of unmanned vehicles, and constructing energy consumption constraint penalty items based on the drone energy consumption model; calculating the gradient of each constraint with respect to the constraint variable;

[0128] The trajectory optimization module is used to solve the optimized path points and corresponding times according to the joint trajectory optimization function of the UAV and the unmanned vehicle and the constraint function of the UAV and the unmanned vehicle, and use the optimized path points and corresponding time to express the final optimized trajectory.

[0129] Embodiment 3

[0130] This embodiment provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the steps in the UAV-UAV joint trajectory optimization method as described above are implemented.

[0131] Embodiment 4

[0132] This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps in the UAV-UAV joint trajectory optimization method as described above are implemented.

[0133] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. The UAV-UAV joint trajectory optimization method is characterized by: The steps include: Generate discrete path points and safe feasible domains for UAVs and unmanned vehicles based on mission requirements and environmental information; Generate reference trajectories according to the starting positions and safe feasible domains of the UAV and the UGV respectively, and represent the reference trajectories as multiple path points and the time corresponding to each path point according to the expected speed; Construct a joint trajectory optimization function for drones and unmanned vehicles; Construct constraint functions for drones and unmanned vehicles, including: constructing obstacle avoidance constraint penalty items based on the safe feasible domains of drones and unmanned vehicles, constructing instantaneous state constraint penalty items based on the dynamic models of drones and unmanned vehicles, imposing height constraints on the position of unmanned vehicles, and constructing energy consumption constraint penalty items based on the drone energy consumption model; calculating the gradient of each constraint with respect to the constraint variable; According to the joint trajectory optimization function of the UAV and the unmanned vehicle and the constraint function of the UAV and the unmanned vehicle, the optimized path points and the corresponding time are solved, and the final optimized trajectory is obtained by using the optimized path points and the corresponding time. Specifically: the path points and time are mapped in time and space to eliminate the constraints in time and space, and the mapped path points and time are used as optimization variables; for the spatial constraints, the vertices of each polyhedron are calculated by using the convex hull algorithm, and any point inside the polyhedron is represented as a convex combination of the polyhedron vertices; then the element-by-element square mapping is used to eliminate the non-negativity constraints and transform the original polyhedron constraints into a new closed ball constraint; finally, smooth surjection is applied to map the points in the unit sphere to the geometric space, so that the points in the polyhedron correspond to the points in the entire geometric space, and the mapped points in the geometric space are used as the coordinate points in the optimization variables.

2. The UAV-UAV joint trajectory optimization method according to claim 1, characterized in that: The joint trajectory optimization function of the UAV and the unmanned vehicle is: ， in, is the flat output of the real-time status of the drone, is the flat output of the real-time status of the unmanned vehicle, is the total task time, is the current time, is a positive diagonal matrix of suitable dimension, is the order of the flat output quantity.

3. The UAV-UAV joint trajectory optimization method according to claim 1, characterized in that: Compute the gradient of each constraint with respect to the constraint variable, including: The obstacle avoidance constraint penalty item is the gradient of the coordinates, the instantaneous state constraint penalty item of velocity, acceleration, attitude angle and thrust is the gradient of the UAV speed, acceleration and jerk, the instantaneous state constraint penalty item of velocity, longitudinal acceleration, tangential acceleration and curvature is the gradient of the unmanned vehicle speed and acceleration; the height constraint penalty item is the gradient of the unmanned vehicle height; the energy consumption constraint penalty item is the gradient of the UAV speed.

4. The UAV-UAV joint trajectory optimization method according to claim 1, characterized in that: The coordinates of the drone or unmanned vehicle at a certain moment or For the safe feasible domain The penalty term for the obstacle avoidance constraint of the hyperplane is set to or : ， ， in, and are the coordinates of the UAV or the unmanned vehicle at a certain moment, and They are the first The normal vector of the hyperplane is and are fixed parameters of the hyperplane.

5. The UAV-UAV joint trajectory optimization method according to claim 1, characterized in that: When constructing the instantaneous state constraint penalty term, it includes: For drones Coordinates of time For example, the speed constraint penalty term is constructed according to the maximum speed constraint, maximum acceleration constraint, maximum pitch angle constraint and maximum thrust constraint. , acceleration constraint penalty , attitude angle constraint penalty and thrust constraint penalty : ， ， ， ， in, , and Representing the drones The velocity, acceleration and jerk at each moment, is the attitude angle of the UAV expressed by the UAV speed, acceleration and jerk, is the propeller thrust of the drone expressed in terms of the drone speed, acceleration and jerk, and is the maximum speed, maximum acceleration, maximum attitude angle and maximum thrust of the UAV; For unmanned vehicles Coordinates of time For example, the speed constraint penalty term is constructed according to the maximum speed constraint, the maximum longitudinal acceleration constraint, the maximum tangential acceleration constraint and the maximum curvature constraint. , longitudinal acceleration constraint penalty , tangential acceleration constraint penalty and the curvature constraint penalty : ， ， ， ， in, and Representing the unmanned vehicle The speed and acceleration of the moment, and are the maximum speed, maximum longitudinal acceleration, maximum tangential acceleration and maximum curvature of the unmanned vehicle, is an auxiliary antisymmetric matrix.

6. The UAV-UAV joint trajectory optimization method according to claim 1, characterized in that: If the mission requirements include a landing mission, it also includes constructing a terminal constraint penalty item for the UAV, and obtaining the gradient of the terminal constraint penalty item for each attitude quantity of the UAV.

7. The UAV-UAV joint trajectory optimization method according to claim 1, characterized in that: The method of solving the optimized path points and corresponding times according to the joint trajectory optimization function of the UAV and the unmanned vehicle and the constraint function of the UAV and the unmanned vehicle comprises the following steps: Perform gradient transfer on constraint variables, converting the gradient of existing penalty items to each constraint variable into the gradient of constraint items to path points and time; The optimization variables and joint trajectory optimization function are put into the solver for solving to obtain the optimized path points and corresponding time.

8. UAV-UAV joint trajectory optimization system, characterized by: include: The initial path generation module is used to generate discrete path points and safe feasible domains for UAVs and unmanned vehicles based on mission requirements and environmental information; Generate reference trajectories according to the starting positions and safe feasible domains of the UAV and the UGV respectively, and represent the reference trajectories as multiple path points and the time corresponding to each path point according to the expected speed; Optimization model building module, used to build joint trajectory optimization functions for drones and unmanned vehicles; Construct constraint functions for drones and unmanned vehicles, including: constructing obstacle avoidance constraint penalty items based on the safe feasible domains of drones and unmanned vehicles, constructing instantaneous state constraint penalty items based on the dynamic models of drones and unmanned vehicles, imposing height constraints on the position of unmanned vehicles, and constructing energy consumption constraint penalty items based on the drone energy consumption model; calculating the gradient of each constraint with respect to the constraint variable; The trajectory optimization module is used to solve the optimized path points and corresponding times according to the joint trajectory optimization function of the UAV and the unmanned vehicle and the constraint function of the UAV and the unmanned vehicle, and use the optimized path points and corresponding times to represent the final optimized trajectory; specifically: the path points and times are mapped in time and space to eliminate the time and space constraints, and the mapped path points and times are used as optimization variables; for the spatial constraints, the vertices of each polyhedron are calculated by using the convex hull algorithm, and any point inside the polyhedron is represented as a convex combination of the polyhedron vertices; then the element-by-element square mapping is used to eliminate the non-negativity constraints and transform the original polyhedron constraints into a new closed ball constraint; finally, smooth surjection is applied to map the points in the unit sphere to the geometric space, so that the points in the polyhedron correspond to the points in the entire geometric space, and the mapped points in the geometric space are used as the coordinate points in the optimization variables.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps in the UAV-UAV joint trajectory optimization method as described in any one of claims 1 to 7 are implemented.

10. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the steps in the UAV-UAV joint trajectory optimization method as described in any one of claims 1-7 are implemented.

Images