Air-ground heterogeneous unmanned system cooperative trajectory planning method for signal-limited environment

Abstract

The present application relates to the trajectory planning and motion control technology field of air-ground cooperative unmanned system, and particularly relates to a signal-restricted environment-oriented air-ground heterogeneous unmanned system cooperative trajectory planning method, comprising: acquiring signal-restricted intersection environment information, unmanned aerial vehicle and unmanned vehicle motion information; determining the observation space and action space of the unmanned aerial vehicle and unmanned vehicle, and a communication perception hybrid reward function; performing reinforcement learning to acquire the joint trajectory of each formation from the initial position to the target position; splicing the joint trajectory of each formation into an initial trajectory; performing inflation obstacle detection to establish a safety corridor; performing smoothing optimization to acquire a global initial trajectory; determining an iterative solution target function; iteratively solving the global initial trajectory to acquire a collision-free trajectory; based on the collision-free trajectory, re-performing optimal control solution to finally acquire a multi-formation cooperative trajectory, and the present application can reduce the unmanned system cooperative trajectory planning calculation amount and improve the success rate.

Description

Technical Field

[0001] This invention relates to the field of trajectory planning and motion control technology for air-ground cooperative unmanned systems, and specifically to a method for cooperative trajectory planning of heterogeneous air-ground unmanned systems in signal-constrained environments. Background Technology

[0002] With the rapid development of autonomous driving technology and intelligent transportation systems, the autonomous management of connected and automated vehicles at unsignalized intersections in urban areas has attracted widespread attention. Existing autonomous management methods for unsignalized intersections heavily rely on vehicle-to-vehicle communication and satellite positioning technologies to obtain accurate vehicle location information and share status data, thereby planning collision-free cooperative driving trajectories. However, in signal-constrained environments such as urban canyons, dense high-rise buildings severely obstruct satellite signal lines of sight, leading to significantly increased positioning drift; simultaneously, direct communication links between ground vehicles are frequently interrupted due to non-line-of-sight obstructions. These two problems render existing unsignalized intersection management frameworks essentially ineffective in signal-constrained scenarios.

[0003] Air-ground cooperative systems offer a new paradigm for solving the aforementioned problems. By equipping each unmanned ground vehicle with a mounted unmanned aerial vehicle (UAV) as a dedicated aerial communication relay node, the UAV takes off when the vehicle approaches an intersection, utilizing its altitude advantage to reconstruct the line-of-sight communication link between the vehicles and obtain accurate positioning information. In the field of air-ground cooperative trajectory planning, existing methods are mainly divided into two categories: decoupled and coupled. Decoupled methods plan the trajectories of the UAV and the ground vehicle independently, with lower computational complexity but difficulty in handling the strong coupling constraints between them. Coupled methods optimize the trajectories of both simultaneously, capable of handling complex interaction constraints, but face problems of high computational cost and difficulty in solving.

[0004] In the prior art, the invention patent with announcement number CN116880434B proposes a collaborative control method for heterogeneous air-ground unmanned swarms based on cloud and fog computing and event triggering mechanisms. However, it does not address the collision avoidance and passage order coordination issues when multiple formations pass through conflict areas such as intersections simultaneously, nor does it consider the impact of line-of-sight propagation probability constraints between air and ground on trajectory planning. The invention patent application with publication number CN115499921A proposes a three-dimensional trajectory design and resource scheduling optimization method for UAV networks, but it does not address the kinematic constraints of ground vehicles and multi-vehicle collision avoidance constraints, and cannot be directly used for multi-formation collaborative trajectory planning. Furthermore, none of the above methods effectively handle the deep coupling problem between heterogeneous kinematic models and highly non-convex line-of-sight probability constraints. If all constraints are directly modeled as an optimal control problem for solution, it will lead to a sharp increase in computational load and a high probability of failure. Summary of the Invention

[0005] In view of the above problems, the present invention provides a cooperative trajectory planning method for air-ground heterogeneous unmanned systems in signal-constrained environments, which solves the technical problems of exponential growth in computation and easy failure in the prior art.

[0006] This invention provides a cooperative trajectory planning method for heterogeneous air-ground unmanned systems in signal-constrained environments, comprising the following steps: Step S1: Obtain environmental information of the signal-restricted intersection, including the location information of roads and obstacles; determine the motion information of the drones and unmanned vehicles, including determining the motion parameters of the drones and unmanned vehicles, as well as the initial position and target position of the formation; wherein each unmanned vehicle carries one drone to form a formation; Step S2: Determine the observation space and action space of the UAV and the unmanned vehicle based on their motion information; A hybrid reward function for communication perception is established based on the distance between the drone and the unmanned vehicle, the distance to obstacles, as well as communication connectivity and line-of-sight probability. Reinforcement learning is performed based on the signal-constrained intersection environment information, the observation space and action space, and the communication-aware hybrid reward function to obtain the joint trajectory from the initial position to the target position of each formation; Step S3: Patch together the joint trajectories of each formation to form an initial trajectory; perform obstacle detection based on the initial trajectory to establish a safe corridor; perform smoothing optimization based on the safe corridor and the maximum trajectory offset threshold to obtain the global initial trajectory; Step S4: Determine the objective function for iterative solution based on kinematic and dynamic constraints; iteratively solve the global initial trajectory to obtain a collision-free trajectory; Each iteration of the solution process includes: performing formation collision detection, determining the corresponding safe window based on whether a collision occurs, performing optimal control based on the safe window and the objective function, and updating the collision-free trajectory; Step S5: Based on the collision-free trajectory and the kinematic constraints, perform the optimal control solution again to finally obtain the multi-formation cooperative trajectory.

[0007] Preferably, in step S1, the motion parameters of the UAV and the unmanned vehicle include the three-dimensional spatial position coordinates, pitch angle, yaw angle and longitudinal velocity of the UAV, pitch and yaw attitude control commands and longitudinal acceleration; and the planar position coordinates, velocity, heading angle and front wheel angle, acceleration, wheelbase and front wheel angle change rate of the unmanned vehicle.

[0008] Preferably, in step S2, the observation space and action space of the UAV and the unmanned vehicle are specifically as follows: The observation space of the unmanned vehicle includes two-dimensional motion state information, Euclidean distance to the nearest static obstacle, Manhattan distance to the target location, and relative spatial distance to the paired aircraft; The observation space of an unmanned vehicle includes three-dimensional motion state information, Euclidean distance to the nearest static obstacle, straight-line distance to the location, and environmental parameters of the air-to-ground propagation model; The motion space of an autonomous vehicle is a continuous motion space, including acceleration control variables and front wheel steering angle change rate control variables; The action space of a drone is a continuous action space, including longitudinal acceleration control, pitch rate control, and yaw rate control.

[0009] Preferably, in step S2, the expression for the communication-aware hybrid reward function is:

[0010]

[0011]

[0012]

[0013]

[0014] in, This represents a communication-aware hybrid reward function. As a spatial distance reward, For safety reasons, As a reward for communication, For smoothness penalty, These are the weighting coefficients for each sub-reward; This indicates the current coordinates of the autonomous vehicle. This indicates the target location coordinates of the autonomous vehicle. Indicates the current position coordinates of the drone. Indicates the coordinates of the forward-guided target position of the UAV, ‖ || represents the Euclidean norm of a vector. k is the penalty gain coefficient. The preset safety margin threshold, The distance between the agent and the nearest static obstacle. and These are the weighting coefficients. Let be the probability of the line-of-sight distance at the current moment. The probability of the line-of-sight distance at the previous moment. The preset acceptable threshold for line-of-sight probability. and These represent the control input vectors for the unmanned vehicle and the drone, respectively.

[0015] Preferably, the step S2, which involves performing reinforcement learning based on the signal-constrained intersection environment information, the observation space and action space, and the communication-aware hybrid reward function to obtain the joint trajectory from the initial position to the target position of each formation, specifically includes: Two execution networks are constructed. The first execution network takes the local observations of the unmanned vehicle as input and outputs the action probability distribution parameters of the unmanned vehicle. The second execution network takes the local observations of the drone as input and outputs the action probability distribution parameters of the drone. The evaluation network is constructed with global state information as input, which includes the complete state of unmanned vehicles and drones in the formation, environmental obstacle information, and mission objective information. The evaluation network outputs the value function estimate under the current global state. The multi-agent proximal policy optimization algorithm is trained based on the two execution networks and the evaluation network. Based on the trained execution network, the trajectory of each formation is extrapolated from its initial position to generate a complete trajectory sequence from the initial position to the target position, which is the joint trajectory.

[0016] Preferably, step S3 specifically includes: Step S3-1: Summarize and stitch together the joint trajectories of each formation according to a unified timeline to obtain the initial global trajectory; Step S3-2: Establish a safe corridor based on the expansion obstacle detection of the UAV, wherein the safe corridor includes the upper and lower bounds of the position coordinates of the UAV and the unmanned vehicle; Step S3-3: Determine the smoothing optimization objective function and constraints. Based on the smoothing optimization objective function, safety corridor, constraints, and maximum trajectory offset threshold, perform smoothing optimization to obtain the global initial trajectory.

[0017] Preferably, in step S3-2, the boundary expression of the safety corridor is:

[0018] in, , Let K and K represent the position coordinates of agent i at time k. Let x and y represent the lower and upper bounds of the x-coordinate of agent i at time k, respectively. Let represent the lower and upper bounds of the y-coordinate of agent i at time k, respectively.

[0019] Preferably, in step S4, the expression of the objective function obtained through iterative solution is:

[0020]

[0021] in, This represents the objective function to be solved iteratively. This represents the objective function for smooth optimization. For the penalty function weights, A parameter representing the degree of violation of dynamic constraints; The total number of formations, and These represent the weights of drones and unmanned vehicles, respectively. and Let represent the control input vectors of the UAV and the unmanned vehicle in the i-th formation, respectively.

[0022] Preferably, in step S4, the step of determining the corresponding safety window based on whether a collision has occurred specifically includes: Static security windows are determined based on the aforementioned security corridor; When a collision is detected in agent i at time k, the static safety window of agent i at time k and its neighboring times is elastically expanded. The expression for the expanded safety window is:

[0023] in, This represents the safety window at time k when a collision occurs. For the elastic scaling factor ( ), This is a static security window. At the moment of collision, For time neighborhood parameters; When it is detected that agent i has not encountered a collision at time k, the static safety window of agent i at time k and its surrounding neighborhood times is tightened. The expression for the tightened safety window is:

[0024] in, This represents the safe window at time k when there is no collision. For the shrinkage parameter ( ), This is the set of collision-free moments.

[0025] Compared with the prior art, the present invention has at least the following beneficial effects: (1) In response to the problems of satellite positioning degradation, vehicle-to-vehicle communication instability and severe line-of-sight occlusion in signal-limited intersections such as urban canyons, this invention proposes a multi-agent reinforcement learning method based on a hybrid reward function of communication perception. The distance constraints between UAVs and unmanned vehicles, the safe distance constraints with obstacles, the communication connectivity constraints, and the line-of-sight probability constraints are integrated into the reward design. This enables the system to automatically generate joint trajectories that meet communication requirements through interactive learning without having to directly solve for height non-convex constraints. This effectively improves the communication reliability and mission executability of air-ground heterogeneous formations in complex occlusion environments.

[0026] (2) This invention adopts a hierarchical processing approach of initial trajectory splicing, expansion obstacle detection, safe corridor construction, and smooth optimization to transform the coarse-grained joint trajectory obtained from reinforcement learning into a global initial trajectory, and further iteratively optimizes it through a safety window mechanism based on collision state selective activation. It expands anti-collision constraints only when there is a collision risk and tightens the safety window when there is no collision, avoiding the over-constraint, deadlock, and solution failure problems caused by the traditional method of forcibly applying dense collision constraints throughout the entire time domain. This significantly reduces the optimization complexity and improves the stability and solution success rate of multi-formation collaborative planning.

[0027] (3) This invention uses the high-quality trajectory generated in the first stage as the initial value for the hot start of the optimal control solution in the second stage, and re-optimizes it in combination with kinematic and dynamic constraints. This allows the collision-free trajectory to meet the actual motion performance requirements of vehicles and UAVs while ensuring safety, thereby achieving trajectory smoothness, executability, and enhanced coordination. This two-stage collaborative optimization mechanism effectively reduces the search space, accelerates the iterative convergence speed, and ultimately outputs a multi-formation collaborative trajectory that balances safety, efficiency, and smoothness, improving the overall robustness and engineering practicality of air-to-ground formation passage in signal-constrained intersection scenarios. Attached Figure Description

[0028] The accompanying drawings are for illustrative purposes only and are not intended to limit the scope of the invention.

[0029] Figure 1 The flowchart shows the collaborative trajectory planning method for air-ground heterogeneous unmanned systems in signal-constrained environments provided by this invention.

[0030] Figure 2 This is a detailed flowchart of the cooperative trajectory planning method for heterogeneous air-ground unmanned systems in signal-constrained environments according to the present invention. Detailed Implementation

[0031] To better understand the above-described objectives, features, and advantages of the present invention, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments of the present invention and the features thereof can be combined with each other. Furthermore, the present invention can be implemented in other ways different from those described herein; therefore, the scope of protection of the present invention is not limited to the specific embodiments disclosed below.

[0032] This invention addresses the technical problem of signal-constrained environments such as urban canyons, where dense building obstruction causes satellite positioning drift and vehicle-to-vehicle communication interruptions, rendering existing signalless intersection management methods ineffective. By equipping each unmanned ground vehicle with a mounted unmanned aerial vehicle (UAV) as a dedicated aerial communication relay node, the UAV takes off when the vehicle approaches the intersection, utilizing its altitude advantage to reconstruct the line-of-sight communication link between vehicles and obtain accurate positioning information. This invention employs a two-stage hierarchical solution framework, integrating multi-agent reinforcement learning with conflict-driven elastic boundary optimization to achieve safe, efficient, and reliable collaborative passage of multiple heterogeneous air-ground formations at signal-constrained, signalless intersections.

[0033] To illustrate the effectiveness of the method proposed in this invention, the following detailed description of the above technical solution is provided through a specific embodiment, such as... Figure 1 , Figure 2 As shown, a cooperative trajectory planning method for air-ground heterogeneous unmanned systems in signal-constrained environments is disclosed. The specific implementation steps are as follows: Step S1: Obtain environmental information of the signal-restricted intersection, including the location information of roads and obstacles; determine the motion information of the drones and unmanned vehicles, including determining the motion parameters of the drones and unmanned vehicles, as well as the initial position and target position of the formation; wherein each unmanned vehicle carries one drone to form a formation; In this step, the present invention first acquires complete environmental information of the signal-controlled intersection, including the location information of roads and obstacles. Specifically, the road information includes the geometric topology of the intersection, the location coordinates of each approach and exit lane, lane width, number of lanes, and road boundary constraints; the obstacle location information can include the spatial distribution of static obstacles within and around the intersection area, including the precise coordinates and envelope dimensions of fixed obstacles such as buildings, roadside facilities, and traffic signs in a two-dimensional plane or three-dimensional space.

[0034] Based on the acquired environmental information, this step further determines the motion information of the drones and unmanned vehicles (UAVs), with each UAV carrying one drone to form an air-to-ground heterogeneous formation unit. The kinematic modeling of the drones and UAVs is described below first.

[0035] This invention uses a simplified point mass kinematics model to describe the spatial motion of the UAV, and the kinematic equations are as follows:

[0036] in, This represents the three-dimensional spatial coordinates of the drone. These represent the pitch angle, yaw angle, and longitudinal velocity of the UAV, respectively. These represent the pitch and yaw attitude control commands and the longitudinal acceleration of the unmanned vehicle, respectively.

[0037] The planar motion of ground vehicles is described using a monorail bicycle model, and the kinematic equations are as follows:

[0038] in, This represents the planar position coordinates of the autonomous vehicle. These represent the speed, steering angle, and front wheel steering angle of the autonomous vehicle, respectively. This indicates the acceleration of the driverless car. This refers to the wheelbase of the autonomous vehicle. This represents the rate of change of the front wheel steering angle of the autonomous vehicle.

[0039] In this step, the present invention obtains the motion parameters of the UAV and the unmanned vehicle based on the above kinematic equations, including: the three-dimensional spatial position coordinates, pitch angle, yaw angle and longitudinal velocity of the UAV, pitch and yaw attitude control commands and longitudinal acceleration; and the planar position coordinates, velocity, heading angle and front wheel angle, acceleration, wheelbase, and front wheel angle change rate of the unmanned vehicle.

[0040] This step also includes determining the initial position and target position of the formation, specifically including determining the initial position and target position of the drone and the unmanned vehicle respectively; in some embodiments, the initial motion state of the drone and the unmanned vehicle and the target motion state can also be determined.

[0041] This step provides the necessary scenario-based information and system constraint boundaries for subsequent trajectory planning, ensuring that the planned trajectory is physically feasible and meets the constraints of the actual application environment.

[0042] Step S2: Determine the observation space and action space of the UAV and the unmanned vehicle based on their motion information; A hybrid reward function for communication perception is established based on the distance between the drone and the unmanned vehicle, the distance to obstacles, as well as communication connectivity and line-of-sight probability. Reinforcement learning is performed based on the signal-constrained intersection environment information, the observation space and action space, and the communication-aware hybrid reward function to obtain the joint trajectory from the initial position to the target position of each formation; In this step, the present invention employs a multi-agent reinforcement learning method to solve the joint trajectory within each formation under a centralized training and distributed execution framework. By relaxing the highly non-convex line-of-sight propagation probability constraint and communication connectivity constraint into soft penalties in the reward function, the agents learn the joint trajectory that satisfies the constraints through trial and error.

[0043] This step first defines the observation space and action space for drones and unmanned vehicles.

[0044] (1) Observation space The observation space of the unmanned vehicle includes two-dimensional motion state information, the Euclidean distance to the nearest static obstacle, the Manhattan distance to the target location, and the relative spatial distance to the paired aircraft. The two-dimensional motion state information includes the aforementioned planar position coordinates, velocity, heading angle, and front wheel steering angle.

[0045] The observation space of the unmanned vehicle includes three-dimensional motion state information, Euclidean distance to the nearest static obstacle, straight-line distance to the location, and environmental parameters of the air-to-ground propagation model. The three-dimensional motion state information includes the aforementioned three-dimensional spatial position coordinates, pitch angle, yaw angle, and longitudinal velocity.

[0046] Through the above steps, this invention defines a comprehensive observation space that includes its own state, environmental perception, and task orientation.

[0047] (2) Action space The motion space of ground vehicles and aircraft directly corresponds to the physical control variables in their respective kinematic models. For unmanned vehicles, the motion space is a continuous motion space, including acceleration control variables and front wheel steering rate control variables; for unmanned aerial vehicles, the motion space is also a continuous motion space, including longitudinal acceleration control variables, pitch rate control variables, and yaw rate control variables.

[0048] All consecutive actions are normalized to the standard interval [-1, 1] before being mapped to the actual physical boundary to ensure the stability of policy learning.

[0049] This invention establishes a communication-aware hybrid reward function, which is composed of a weighted linear combination of multiple sub-reward items, and its expression is:

[0050] in, This represents a communication-aware hybrid reward function. As a spatial distance reward, For safety reasons, As a reward for communication, For smoothness penalty, The weighting coefficients for each sub-reward.

[0051] The spatial distance reward is used to drive the unmanned vehicle to approach the target position and guide the drone to maintain a reasonable leading distance. Its expression is:

[0052] in, This indicates the current coordinates of the autonomous vehicle. This indicates the target location coordinates of the autonomous vehicle. Indicates the current position coordinates of the drone. Indicates the coordinates of the forward-guided target position of the UAV, ‖ ‖ represents the Euclidean norm of the vector. The reward consists of two parts: the first is the negative distance between the current position of the unmanned vehicle and its target position, with a larger reward for closer distances, used to drive the unmanned vehicle to continuously approach the target; the second is the negative distance between the current position of the drone and its leading guidance target position, which is determined by a preset leading guidance distance offset from the current position of the unmanned vehicle along the target direction, used to guide the drone to always remain in an appropriate position in front of the unmanned vehicle to provide effective communication relay coverage.

[0053] The safety penalty is designed based on the concept of an artificial potential field to guide unmanned vehicles and drones away from static obstacles. Specifically, a safety margin threshold is defined; when the distance between the agent and the nearest obstacle is less than the safety margin threshold, a safety penalty is activated. The expression for the safety penalty is:

[0054] Where k is the penalty gain coefficient. The preset safety margin threshold, This represents the distance between the agent and the nearest static obstacle. When the distance between the agent and the obstacle is less than the safety margin threshold, a secondary penalty is activated to prevent the agent from moving away from the obstacle; the closer the distance, the greater the penalty. When the distance is greater than or equal to the safety margin threshold, no penalty is applied.

[0055] The communication reward, constructed based on line-of-sight probability, guides the UAV to move towards a spatial pose with a higher line-of-sight probability, thereby ensuring the reliability of the air-to-ground communication link. The communication reward consists of two parts: a temporal difference reward based on the line-of-sight probability and a line-of-sight probability threshold penalty, expressed as follows:

[0056] in, and These are the weighting coefficients. Let be the probability of the line-of-sight distance at the current moment. The probability of the line-of-sight distance at the previous moment. The preset acceptable threshold for line-of-sight probability is defined as follows: The first term is a dense reward, which uses the difference between the line-of-sight probability at the current moment and the previous moment as a reward signal to encourage the UAV to move towards a spatial pose with a higher line-of-sight probability; the second term is a hinge penalty, which is activated only when the line-of-sight probability is lower than the preset threshold, and applies a linear penalty to the portion below the threshold.

[0057] The line-of-sight probability in the aforementioned communication reward The calculation is based on the air-to-ground statistical propagation model, and its expression is:

[0058]

[0059] in, Let α and β represent the line-of-sight probability, and exp( ) represents the natural index, θ represents the common belief angle for drones and unmanned vehicles, and arctan( ) represents the arctangent function. and These represent the vertical height coordinates of the drone and the unmanned vehicle, respectively. and These represent the position coordinates of the drone and the unmanned vehicle in the horizontal plane, respectively.

[0060] Through the above communication reward expression, this invention relaxes the highly non-convex line-of-sight propagation probability constraint and communication connectivity constraint into an optimizable reward signal, enabling the agent to automatically learn a joint trajectory that meets the communication reliability requirements through trial and error exploration, without having to directly solve the non-convex constraints numerically.

[0061] The smoothness penalty is used to suppress drastic changes in the control variable, and its expression is:

[0062] in, and Let represent the control input vectors of the autonomous vehicle and the drone, respectively. The penalty term above takes the negative value of the sum of the squares of the control inputs; the larger the control input, the heavier the penalty, thereby encouraging the generation of smooth and energy-efficient trajectories.

[0063] The smoothness reward weight is set to 0.1 in the first stage, which is much smaller than the communication reward weight and the safety penalty weight, so that the strategy prioritizes communication and obstacle avoidance constraints. The smoothness of the trajectory is further improved through optimization in the second stage.

[0064] Subsequently, this invention employs a multi-agent proximal policy optimization method within a centralized training and distributed execution framework for reinforcement learning, acquiring the joint trajectory of each formation from its initial position to its target position. Specifically, the centralized training and distributed execution framework includes two types of neural network structures: an execution network and an evaluation network. Specifically, two independent execution networks are constructed. The first execution network takes the local observations of the unmanned vehicle (UAV) as input and outputs the action probability distribution parameters of the UAV. The second execution network takes the local observations of the unmanned aerial vehicle (UAV) as input and outputs the action probability distribution parameters of the UAV. Since the action space is a continuous space, the action probability distribution is typically represented by a Gaussian distribution, and the execution network outputs the mean and standard deviation parameters of this Gaussian distribution. A centralized evaluation network is constructed. This evaluation network takes global state information as input, including the complete states of the UAVs and UAVs within the formation, environmental obstacle information, and mission objective information. The evaluation network outputs an estimate of the value function under the current global state, used to evaluate the quality of the joint state action pairs.

[0065] Through iterative training of the multi-agent proximal policy optimization algorithm, the unmanned vehicles and drones in the formation gradually learn cooperative strategies.

[0066] After training, the learned execution network is used to perform trajectory extrapolation for each formation starting from its initial position, generating a complete trajectory sequence from the initial position to the target position, which serves as the joint trajectory.

[0067] The joint trajectory includes state variables such as the position, speed, orientation angle, and front wheel rotation angle of the unmanned vehicle at discrete time steps, as well as state variables such as the three-dimensional position, pitch angle, yaw angle, and speed of the UAV, and also includes the corresponding control sequence.

[0068] This step outputs the joint trajectory within each air-to-ground heterogeneous formation. The joint trajectory within the formation includes the initial driving trajectory of each unmanned vehicle, the initial flight trajectory of each paired UAV, and the corresponding control sequence.

[0069] Since the first stage only considers the constraints within each formation and does not consider the collision avoidance between different formations, the resulting trajectory is feasible within the formation, but there may be spatiotemporal conflicts between different formations, which requires inter-formational collaborative optimization in subsequent steps.

[0070] Step S3: Patch together the joint trajectories of each formation to form an initial trajectory; perform obstacle detection based on the initial trajectory to establish a safe corridor; perform smoothing optimization based on the safe corridor and the maximum trajectory offset threshold to obtain the global initial trajectory; In this step, the joint trajectories of each formation are stitched together to form the initial trajectory, and a global initial trajectory with good smoothness is generated by establishing a safe corridor and smoothing optimization.

[0071] Step S3-1: Summarize and stitch together the joint trajectories of each formation according to a unified timeline to obtain the initial global trajectory; In this step, the joint trajectories of each formation are summarized and spliced ​​together according to a unified timeline to form an initial global trajectory that includes all unmanned vehicles and drones.

[0072] Step S3-2: Establish a safe corridor based on the expansion obstacle detection of the UAV, wherein the safe corridor includes the upper and lower bounds of the position coordinates of the UAV and the unmanned vehicle; The safe corridor mentioned in this invention refers to a local collision-free feasible space constructed along the trajectory at each discrete moment, which is used to constrain the trajectory offset range in the subsequent optimization process.

[0073] Specifically, the first step is to perform obstacle expansion processing. For the drone, considering its movement in three-dimensional space, the drone is modeled as having a radius of... A sphere is used to uniformly expand static obstacles in the original environment according to its radius, generating an expanded obstacle map.

[0074] For each discrete time k and each agent i, the boundary of the safety corridor is expanded outward along each coordinate axis, centered on the agent's trajectory position at time k. The expansion process adopts an iterative growth method: starting from the current position, it expands gradually along the positive and negative x-directions, positive and negative y-directions (and positive and negative z-directions for UAVs) with a preset step size Δ. After each expansion, collision detection is performed on the expanded obstacle map. If a collision is detected, the expansion in that direction stops; otherwise, the expansion continues until the preset maximum expansion distance is reached.

[0075] Through the above iterative expansion, an axis-aligned bounding box is determined for each agent at each time k, forming the safe corridor. The boundary expression of the safe corridor is:

[0076] in, , Let K and K represent the position coordinates of agent i at time k. Let x and y represent the lower and upper bounds of the x-coordinate of agent i at time k, respectively. Let represent the lower and upper bounds of the y-coordinate of agent i at time k, respectively.

[0077] Step S3-3: Determine the smoothing optimization objective function and constraints. Based on the smoothing optimization objective function, safety corridor, constraints, and maximum trajectory offset threshold, perform smoothing optimization to obtain the global initial trajectory.

[0078] In this step, the present invention determines the smoothing optimization objective function and constraints. The smoothing optimization objective function is set to minimize the sum of squares of the control inputs of all agents over the entire time domain, in order to achieve trajectory smoothness and energy efficiency.

[0079] The constraints of the optimization process include: the UAV and the unmanned vehicle satisfy their respective kinematic equations; the optimized trajectory must remain within the boundary of the corresponding safety corridor at each time step; and the maximum deviation of the optimized trajectory from the initial global trajectory must not exceed the maximum expansion threshold. .

[0080] In some embodiments, the constraints also include the spatial distance between the drone and the aircraft not exceeding a preset threshold. .

[0081] The global initial trajectory is obtained by optimizing the objective function and constraints described above. The smooth optimization problem can be solved using existing nonlinear optimization algorithms, and this invention does not limit the application of such algorithms.

[0082] After smoothing optimization, the global initial trajectory is obtained. The global initial trajectory satisfies all system constraints except for the collision constraints between formations. At this time, there may still be dynamic collision risks between different formations. In subsequent steps, formation collaborative optimization is needed to eliminate potential conflicts.

[0083] Step S4: Determine the objective function for iterative solution based on kinematic and dynamic constraints; iteratively solve the global initial trajectory to obtain a collision-free trajectory; Each iteration of the solution process includes: performing formation collision detection, determining the corresponding safe window based on whether a collision occurs, performing optimal control based on the safe window and the objective function, and updating the collision-free trajectory; This step utilizes a conflict-driven elastic boundary mechanism to iteratively solve the intermediate optimal control problem, gradually eliminating dynamic collisions between different formations and obtaining collision-free trajectories.

[0084] (1) Determine the objective function for iterative solution based on kinematic and dynamic constraints.

[0085] This invention establishes an iterative solution objective function. The objective function consists of two parts: the first part is the original performance index, used to minimize the weighted sum of squares of the control inputs; the second part is a penalty function term for violations of kinematic and dynamic constraints, used to soften some nonlinear constraints before incorporating them into the objective function. The expression of the objective function is:

[0086]

[0087] in, This represents the objective function to be solved iteratively. This represents the objective function for smooth optimization. For the penalty function weights, A parameter representing the degree of violation of dynamic constraints; It represents the degree of violation of kinematic and dynamic constraints, specifically quantified as the sum of squared residuals of the UAV kinematic equations and the unmanned vehicle kinematic equations at each discrete time. That is, the summation of the squared difference between the actual state change of the UAV and the unmanned vehicle at each discrete time and the theoretical state change calculated by the kinematic equations over all discrete time points and all formations. The total number of formations, and These represent the weighting coefficients for the control inputs of the drone and the unmanned vehicle, respectively. and Let represent the control input vectors of the UAV and the unmanned vehicle in the i-th formation, respectively.

[0088] By using the above objective function, this invention reduces the number and complexity of explicit constraints and improves the robustness of the optimization problem.

[0089] (2) The global initial trajectory is solved iteratively. Each iteration includes formation collision detection, safety window determination and optimal control solution.

[0090] First, collision detection between formations is performed on the current trajectory. The collision detection is carried out step by step in discrete time steps. At each time k, it is detected whether there is a spatial conflict between agents in different formations.

[0091] In some embodiments, the relative position and distance between the drone and the unmanned vehicle can be used to determine whether a formation collision has occurred.

[0092] Subsequently, based on the collision detection results, a conflict-driven elastic boundary mechanism is used to determine the safety window for each agent at each time step. In this step, the present invention first determines the static safety window, which can be set to be consistent with the safety corridor in the preceding steps.

[0093] When a collision is detected in agent i at time k, the static safety window of agent i at time k and its neighboring times is elastically expanded. The expression for the expanded safety window is:

[0094] in, This represents the safety window at time k when a collision occurs. For the elastic scaling factor ( ), This is a static security window. At the moment of collision, For time neighborhood parameters.

[0095] When it is detected that agent i has not encountered a collision at time k, the static safety window of agent i at time k and its surrounding neighborhood times is tightened. The expression for the tightened safety window is:

[0096] in, This represents the safe window at time k when there is no collision. For the shrinkage parameter ( ), This is the set of collision-free moments.

[0097] Through the above steps, a spatiotemporal buffer is allocated before and after the collision, and the solver has sufficient margin to perform early avoidance maneuvers. The buffer after the collision ensures a smooth trajectory regression that is kinematically feasible. At collision-free moments, the safety window is tightened to closely follow the baseline trajectory, reduce unnecessary deviations, and maintain initial optimality.

[0098] Subsequently, based on the aforementioned safety window and objective function, the present invention performs optimal control solution under constraints.

[0099] The constraints may include: the UAV and the unmanned vehicle satisfying their respective kinematic equations; the optimized trajectory remaining within the boundary of the corresponding safety corridor at each time step; and the maximum deviation of the optimized trajectory from the initial global trajectory not exceeding the maximum expansion threshold. And the spatial distance between the drone and the aircraft does not exceed a preset threshold. wait.

[0100] The solution process continuously updates the current trajectory until the convergence criterion is met or the maximum number of iterations is reached, at which point the solution ends and the latest collision-free trajectory is obtained.

[0101] Through the above iterative solution process, the collision-free trajectory output in this step eliminates conflicts between different formations while maintaining the reliability of communication and the feasibility of approximate motion among the formations.

[0102] Step S5: Based on the collision-free trajectory and the kinematic constraints, perform the optimal control solution again to finally obtain the multi-formation cooperative trajectory.

[0103] In this step, the present invention uses the collision-free trajectory output from the aforementioned steps as the initial value for solving. Since the collision-free trajectory has eliminated inter-formation conflicts and is close to the feasible region, the optimization solver can quickly converge to the optimal solution or near-optimal solution that satisfies all constraints.

[0104] The final multi-formation cooperative trajectory includes the driving trajectory of each unmanned vehicle, the flight trajectory of each unmanned aerial vehicle, and the corresponding control sequence.

[0105] Through the hierarchical collaborative planning framework provided by this invention, the invention successfully solves the joint trajectory planning problem of heterogeneous unmanned systems in air-ground environments with limited signal coverage, and realizes safe, efficient and reliable collaborative passage of multiple formations at intersections without signal coverage.

[0106] While the specific embodiments of the present invention depict actions or steps in a particular order, this should be understood as requiring such actions or steps to be performed in the specific order shown or in sequential order, or requiring all illustrated actions or steps to be performed to achieve the desired result. In certain environments, multitasking and parallel processing may be advantageous. Similarly, although several specific implementation details are included in the above discussion, these should not be construed as limiting the scope of this disclosure. Certain features described in the context of individual embodiments may also be implemented in combination in a single implementation. Conversely, various features described in the context of a single implementation may also be implemented individually or in any suitable sub-combination in multiple implementations.

[0107] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A cooperative trajectory planning method for air-to-ground heterogeneous unmanned systems in signal-constrained environments, characterized in that, Includes the following steps: Step S1: Obtain environmental information of the signal-restricted intersection, including the location information of roads and obstacles; determine the motion information of the drones and unmanned vehicles, including determining the motion parameters of the drones and unmanned vehicles, as well as the initial position and target position of the formation; wherein each unmanned vehicle carries one drone to form a formation; Step S2: Determine the observation space and action space of the UAV and the unmanned vehicle based on their motion information; A hybrid reward function for communication perception is established based on the distance between the drone and the unmanned vehicle, the distance to obstacles, as well as communication connectivity and line-of-sight probability. Reinforcement learning is performed based on the signal-constrained intersection environment information, the observation space and action space, and the communication-aware hybrid reward function to obtain the joint trajectory from the initial position to the target position of each formation; Step S3: Patch together the joint trajectories of each formation to form an initial trajectory; perform obstacle detection based on the initial trajectory to establish a safe corridor; perform smoothing optimization based on the safe corridor and the maximum trajectory offset threshold to obtain the global initial trajectory; Step S4: Determine the objective function for iterative solution based on kinematic and dynamic constraints; The global initial trajectory is iteratively solved to obtain a collision-free trajectory; Each iteration of the solution process includes: performing formation collision detection, determining the corresponding safe window based on whether a collision occurs, performing optimal control based on the safe window and the objective function, and updating the collision-free trajectory; Step S5: Based on the collision-free trajectory and the kinematic constraints, perform the optimal control solution again to finally obtain the multi-formation cooperative trajectory.

2. The cooperative trajectory planning method for air-to-ground heterogeneous unmanned systems in signal-constrained environments according to claim 1, characterized in that, In step S1, the motion parameters of the UAV and the unmanned vehicle include the three-dimensional spatial position coordinates, pitch angle, yaw angle and longitudinal velocity of the UAV, pitch and yaw attitude control commands and longitudinal acceleration; and the planar position coordinates, velocity, heading angle and front wheel angle, acceleration, wheelbase and front wheel angle change rate of the unmanned vehicle.

3. The cooperative trajectory planning method for air-to-ground heterogeneous unmanned systems in signal-constrained environments according to claim 2, characterized in that, In step S2, the observation space and action space of the drone and the unmanned vehicle are specifically as follows: The observation space of the unmanned vehicle includes two-dimensional motion state information, Euclidean distance to the nearest static obstacle, Manhattan distance to the target location, and relative spatial distance to the paired aircraft; The observation space of an unmanned vehicle includes three-dimensional motion state information, Euclidean distance to the nearest static obstacle, straight-line distance to the location, and environmental parameters of the air-to-ground propagation model; The motion space of an autonomous vehicle is a continuous motion space, including acceleration control variables and front wheel steering angle change rate control variables; The action space of a drone is a continuous action space, including longitudinal acceleration control, pitch rate control, and yaw rate control.

4. The cooperative trajectory planning method for air-to-ground heterogeneous unmanned systems in signal-constrained environments according to claim 3, characterized in that, In step S2, the expression for the communication-aware hybrid reward function is: in, This represents a communication-aware hybrid reward function. As a spatial distance reward, For safety reasons, As a reward for communication, For smoothness penalty, These are the weighting coefficients for each sub-reward; This indicates the current coordinates of the autonomous vehicle. This indicates the target location coordinates of the autonomous vehicle. Indicates the current position coordinates of the drone. Indicates the coordinates of the forward-guided target position of the UAV, ‖ ‖ denotes the Euclidean norm of the vector, and k is the penalty gain coefficient. The preset safety margin threshold, The distance between the agent and the nearest static obstacle. and These are the weighting coefficients. The probability of line-of-sight distance at the current moment. The probability of the line-of-sight distance at the previous moment. The preset acceptable threshold for line-of-sight probability. and These represent the control input vectors for the unmanned vehicle and the drone, respectively.

5. The cooperative trajectory planning method for air-to-ground heterogeneous unmanned systems in signal-constrained environments according to claim 4, characterized in that, In step S2, the step of performing reinforcement learning based on the signal-constrained intersection environment information, the observation space and action space, and the communication-aware hybrid reward function to obtain the joint trajectory from the initial position to the target position of each formation specifically includes: Two execution networks are constructed. The first execution network takes the local observations of the unmanned vehicle as input and outputs the action probability distribution parameters of the unmanned vehicle. The second execution network takes the local observations of the drone as input and outputs the action probability distribution parameters of the drone. The evaluation network is constructed with global state information as input, which includes the complete state of unmanned vehicles and drones in the formation, environmental obstacle information, and mission objective information. The evaluation network outputs the value function estimate under the current global state. The multi-agent proximal policy optimization algorithm is trained based on the two execution networks and the evaluation network. Based on the trained execution network, the trajectory of each formation is extrapolated from its initial position to generate a complete trajectory sequence from the initial position to the target position, which is the joint trajectory.

6. The cooperative trajectory planning method for air-to-ground heterogeneous unmanned systems in signal-constrained environments according to claim 5, characterized in that, Step S3 specifically includes: Step S3-1: Summarize and stitch together the joint trajectories of each formation according to a unified timeline to obtain the initial global trajectory; Step S3-2: Establish a safe corridor based on the expansion obstacle detection of the UAV, wherein the safe corridor includes the upper and lower bounds of the position coordinates of the UAV and the unmanned vehicle; Step S3-3: Determine the smoothing optimization objective function and constraints. Based on the smoothing optimization objective function, safety corridor, constraints, and maximum trajectory offset threshold, perform smoothing optimization to obtain the global initial trajectory.

7. The cooperative trajectory planning method for air-to-ground heterogeneous unmanned systems in signal-constrained environments according to claim 6, characterized in that, In step S3-2, the boundary expression of the safety corridor is: in, , Let K and K represent the position coordinates of agent i at time k. Let x and y represent the lower and upper bounds of the x-coordinate of agent i at time k, respectively. Let represent the lower and upper bounds of the y-coordinate of agent i at time k, respectively.

8. The cooperative trajectory planning method for air-to-ground heterogeneous unmanned systems in signal-constrained environments according to claim 7, characterized in that, In step S4, the expression for the objective function obtained through iterative solution is: in, This represents the objective function to be solved iteratively. This represents the objective function for smooth optimization. For the penalty function weights, A parameter representing the degree of violation of dynamic constraints; The total number of formations, and These represent the weights of drones and unmanned vehicles, respectively. and Let represent the control input vectors of the UAV and the unmanned vehicle in the i-th formation, respectively.

9. The cooperative trajectory planning method for air-to-ground heterogeneous unmanned systems in signal-constrained environments according to claim 8, characterized in that, In step S4, the step of determining the corresponding safety window based on whether a collision has occurred specifically includes: Static security windows are determined based on the aforementioned security corridor; When a collision is detected in agent i at time k, the static safety window of agent i at time k and its neighboring times is elastically expanded. The expression for the expanded safety window is: in, This represents the safety window at time k when a collision occurs. This is the flexible scaling factor. This is a static security window. At the moment of collision, For time neighborhood parameters; When it is detected that agent i has not encountered a collision at time k, the static safety window of agent i at time k and its surrounding neighborhood times is tightened. The expression for the tightened safety window is: in, This represents the safe window at time k when there is no collision. For the shrinkage parameter, It is the set of collision-free moments.

Images