Adaptive hierarchical multi-unmanned vehicle cooperative motion planning method under three-dimensional space-time constraints

Abstract

The disclosure provides a three-dimensional space-time constraint adaptive layered multi-unmanned vehicle cooperative motion planning method, which constructs a behavior planning layer, a three-dimensional space-time constraint layer and a trajectory optimization layer; the behavior planning layer and the three-dimensional space-time constraint layer are arranged in a central controller, the behavior planning layer generates a globally optimal reference trajectory of each unmanned vehicle; the three-dimensional space-time constraint layer generates a three-dimensional space-time safety interval of each unmanned vehicle according to the reference trajectory generated by the behavior planning layer, so as to provide a suitable and as large as possible space range close to the reference trajectory for trajectory optimization of the trajectory optimization layer; the trajectory optimization layer is arranged in each unmanned vehicle, and based on local perception, a local planning is implemented on the reference trajectory under the constraint of a vehicle model and the three-dimensional space-time safety interval, so as to generate an optimal motion trajectory for control tracking in real time. The method can significantly improve the cooperative efficiency and safety of the vehicle team in a complex dynamic environment.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of robots, and relates to a self-adaptive hierarchical multi-unmanned vehicle cooperative motion planning method under three-dimensional space-time constraints. BACKGROUND

[0002] With the rapid development of unmanned driving technology, especially in the fields of intelligent transportation, autonomous vehicle fleet, logistics and disaster rescue, multi-unmanned vehicle cooperative motion planning has become an important research direction. Multi-vehicle cooperative motion planning not only requires accurate control of single vehicle motion, but also needs to achieve efficient cooperation and coordination between vehicles. How to improve overall efficiency while ensuring safety has become the core challenge of current technical research.

[0003] Currently, multi-vehicle cooperative motion planning is mainly divided into centralized and distributed architectures. The centralized method relies on a central controller to provide a global optimal solution, but the computational burden is heavy, making it difficult to adapt to large-scale and high real-time scenarios. The distributed method improves scalability and response speed through distributed computing, but lacks global coordination and is prone to local optimization. Although the hierarchical algorithm optimizes the two, it generates multiple branches when handling conflicts, each branch solves the conflict path separately, the node expands, the calculation time is high, and the adaptability is insufficient.

[0004] In addition, existing obstacle avoidance techniques have low computational efficiency in high-density traffic, making it difficult to meet real-time requirements, increasing the risk of collision and reducing the efficiency of the vehicle fleet. Many methods do not fully consider vehicle dynamics, resulting in poor obstacle avoidance effect in dynamic environments and affecting safety.

[0005] In terms of path and speed optimization, existing methods often separate path and speed planning, lack coordination mechanisms, and result in trajectory and speed mismatch, affecting vehicle stability and control accuracy. Simplified planning models do not fully consider dynamic constraints, resulting in infeasible planning results.

[0006] Despite some progress, existing technologies still face challenges such as dynamic obstacle avoidance and balancing global and local optimization. Traditional single-vehicle planning methods cannot meet the needs of multi-vehicle cooperation, especially in rapidly changing environments. Therefore, there is an urgent need for an efficient and universal cooperative planning method that can effectively coordinate behavior decision-making and trajectory optimization in complex scenarios, improving cooperative efficiency, safety and adaptability. SUMMARY

[0007] Therefore, the application provides a self-adaptive hierarchical multi-unmanned vehicle cooperative motion planning method under three-dimensional space-time constraints, which can significantly improve the cooperative efficiency and safety of the vehicle fleet in complex dynamic environments.

[0008] To solve the above technical problems, the application is implemented as follows.

[0009] A three-dimensional space-time constraint adaptive hierarchical multi-unmanned vehicle cooperative motion planning method comprises:

[0010] A behavior planning layer, a three-dimensional space-time constraint layer and a trajectory optimization layer are constructed; the behavior planning layer and the three-dimensional space-time constraint layer are arranged in a central controller and are used for realizing global planning and cooperative coordination; and the trajectory optimization layer is arranged in each unmanned vehicle and is used for generating an optimal motion trajectory conforming to a dynamic constraint;

[0011] The behavior planning layer generates a dynamic behavior primitive based on a vehicle dynamic characteristic; and an optimal dynamic behavior primitive sequence is screened in a feasible domain limit of global map information, so as to generate a globally optimal reference trajectory of each unmanned vehicle;

[0012] The three-dimensional space-time constraint layer generates a three-dimensional space-time safety interval of each unmanned vehicle according to the reference trajectory generated by the behavior planning layer and in combination with an environment and a vehicle state; the three-dimensional space-time safety interval is composed of drivable intervals corresponding to each time period on the trajectory; when the three-dimensional space-time safety interval is generated, a driving interval range breadth and a small deviation degree of a key point reference trajectory of the driving interval are taken as generation targets, and meanwhile, the generation is limited to not interfering with a three-dimensional space-time environment and multiple vehicles not occupying a same space position at a same time period;

[0013] The trajectory optimization layer implements local planning on the reference trajectory based on a local perception under a constraint of a vehicle model and the three-dimensional space-time safety interval, so as to generate an optimal motion trajectory for control tracking in real time.

[0014] Preferably, in the behavior planning layer, the dynamic behavior primitive generated based on the vehicle dynamic characteristic is:

[0015] The second-order state differential equation of the unmanned vehicle is combined with a nonlinear term to form an expression of the dynamic behavior primitive:

[0016]

[0017] Wherein, y is a vehicle state, g is a target state of a task, τ is a control trajectory speed, and α y and β y are gain coefficients, and f(x) is a nonlinear term for adjusting a trajectory shape and is expressed by a weighted sum of basis functions.

[0018] Preferably, in the behavior planning layer, the optimal dynamic behavior primitive sequence is screened in a feasible domain limit of global map information, so as to generate a globally optimal reference trajectory of each unmanned vehicle:

[0019] A behavior primitive cost function is designed and includes a node cost C n (n) and an edge cost C e (e pq );

[0020] Node cost C n (n) is measured by the difference between node n in the trajectory and the target point;

[0021] Edge cost C e pq (e) is measured by the trajectory smoothness, task execution, human-like driving features and overall traffic efficiency of the trajectory e pq between node p and node q in the trajectory;

[0022] The mixed integer linear programming MILP is adopted to jointly optimize the dynamic behavior primitive sequence of each unmanned vehicle based on the node cost C n (n) and the edge cost C e (e pq ), so as to form the globally optimal reference trajectory of each unmanned vehicle.

[0023] Preferably, the node cost Cn(n) is:

[0024] C n (n) = ω distance ·dist(s n , s target ) + ω heading ·Δθ(s n , s target )

[0025] Wherein, n represents the nth node in the trajectory, s n is the state of node n in the trajectory, s target is the state of the target point in the trajectory; dist(s n , s target ) is the distance deviation, Δθ(s n , s target ) is the heading deviation, both of which are non-negative; ω distance and ω heading represent the distance deviation weight and the heading deviation weight;

[0026] The edge cost C e (e pq ) is:

[0027] C e (e pq ) = ω smooth ·J P + ω task ·C n (n q ) + ω human ·C h (e pq ) + ω efficient ·C v ||v e || ​

[0028] wherein e pq represents an edge between nodes n p and n q ; J P is a path smoothness; C n (n q ) represents a node cost of node n q ; C h (e pq ) represents a human-like driving feature, which is obtained according to an angle deviation of node n q to node n p , and a human-like driving feature lookup table; C v ||v e || represents an overall traffic efficiency cost, which is negatively correlated with a scalar ||v e ||; ||v e || represents an average speed of edge e pq ; ω smooth , ω task , ω human and ω efficient represent weights of corresponding items;

[0029] The path smoothness J P in the edge cost C pq (e e ) is represented as:

[0030]

[0031] wherein t s and t g are a start point and an end point of a path corresponding to e pq ; v(t) is a rear axle speed of the unmanned vehicle, δ(t) is a front wheel steering angle of the vehicle, and L is a wheelbase of the vehicle; c s is a steering control amount, and ω z is a yaw rate, and by minimizing an integral of the steering control amount and the yaw rate, the overall curvature of the trajectory is ensured to be minimum.

[0032] Within the feasible domain limit of the global map information, a total objective function composed of the node cost, the edge cost and the path smoothness is minimized as an objective, and a MILP model is used to jointly optimize dynamic behavior primitive sequences of all unmanned vehicles to generate globally optimal reference trajectories of the unmanned vehicles.

[0033] Preferably, in the three-dimensional space-time constraint layer, a three-dimensional space-time safety interval is generated in the following manner:

[0034] The reference trajectory planned by the behavior planning layer is resampled into K time periods according to local computing resources, and the kth time period is denoted as time period

[0035] Construct key performance indicators, including driving range range and deviation degree from reference trajectory

[0036] The driving range range is:

[0037]

[0038] Wherein, k represents the time period number; represents the range range weight of unmanned vehicle i; time period The drivable range area of time period is characterized by is the maximum and minimum value of the drivable range x, y direction; by maximizing Ensure that the planned drivable range area is overall maximum;

[0039] The deviation degree from the reference trajectory is:

[0040]

[0041] Wherein, is the deviation degree weight of unmanned vehicle i, the coordinates (x i (k), y i (k)) are the coordinates of the specified key points in the corresponding drivable range of time period , and the coordinates are the coordinates of the reference trajectory sampling time points in time period ; by minimizing Ensure that the planned drivable range is close to the reference trajectory;

[0042] The optimization generation of three-dimensional space-time safety range integrates the cost of all unmanned vehicles, and the final objective function is represented as:

[0043]

[0044] Wherein, η i is the weight of unmanned vehicle i, indicating the priority;

[0045] Take J corridor min as the goal, while limiting not to interfere with the three-dimensional space-time environment and multiple vehicles are not allowed to occupy the same space position at the same time period, jointly optimize the drivable range of all unmanned vehicles, all time periods , and obtain the three-dimensional space-time safety range of all unmanned vehicles;

[0046] The resampled reference trajectory, together with the three-dimensional space-time safety interval, is sent to the trajectory optimization layer of the corresponding unmanned vehicle.

[0047] Preferably, in the trajectory optimization layer, the reference trajectory is locally planned as:

[0048] The global path feasibility constraint and the local path deviation rate constraint are constructed; the global path feasibility is maximized to ensure that the planned path meets the vehicle capability and ride comfort; and the local path deviation rate is minimized to ensure that the planned path is close to the reference trajectory issued by the three-dimensional space-time constraint layer;

[0049] According to the current environmental complexity and the current communication bandwidth, the weight of the global path feasibility constraint and the weight of the local path deviation rate constraint are dynamically adjusted;

[0050] The global path feasibility constraint and the local path deviation rate constraint are weighted by the weights to obtain a total objective function of the optimal motion trajectory of the unmanned vehicle;

[0051] The time periods in the reference trajectory issued by the three-dimensional space-time constraint layer are jointly optimized to obtain the optimal motion trajectory, with the total objective function of the optimal motion trajectory being minimized as the goal.

[0052] Preferably, the global path feasibility constraint includes a trajectory smoothness constraint and a trajectory comfort constraint

[0053] The trajectory smoothness constraint is:

[0054]

[0055] wherein t represents a sampling time point of the trajectory optimization layer, represents a trajectory smoothness weight of the unmanned vehicle i; is a front wheel steering angle, and L is a vehicle wheelbase;

[0056] The trajectory comfort constraint is:

[0057]

[0058] wherein represents a weight of a front wheel steering angle change rate of the unmanned vehicle i, represents a weight of an acceleration change rate of the unmanned vehicle i;

[0059] The local path deviation rate constraint is:

[0060]

[0061] in, This represents the time period in the reference trajectory of the autonomous vehicle i issued by the trajectory optimization layer to the three-dimensional spatiotemporal constraint layer. The node coordinates obtained from local path planning are the objects that need to be optimized; (x i (k),y i (k) represents Coordinates of key points within the drivable area; and These represent the deviation weights of autonomous vehicle i from key points in the x-axis and y-axis directions, respectively; by minimizing the local path deviation rate... This ensures that the planned local path closely follows the reference trajectory within the drivable area.

[0062] Preferably, the weight W of the global path feasibility constraint centralized Weight W of the deviation from the local path constraint distributed They are respectively:

[0063]

[0064] W distributed =1-W centralized

[0065] Among them, C env Given the complexity of the current environment, C max B is the maximum threshold for environmental complexity. comm B represents the current communication bandwidth. max This represents the maximum communication bandwidth.

[0066] Preferably, W distributed W is updated at each sampling time t as it changes over time. distributed The updated formula is:

[0067] W centralized (t)=W centralized (t-1)+ΔW·e -λt

[0068] Among them, W centralized (t) represents the updated weight value at time t, W centralized (t-1) represents the weight value used at time t-1; ΔW = W target -W centralized (t-1) represents the weight value at time t-1 and the target weight value W calculated based on environmental complexity and communication bandwidth. target The difference between them; λ is the convergence rate coefficient.

[0069] Preferably, a cache mechanism is further arranged between the three-dimensional space-time constraint layer and the trajectory optimization layer, including success cache and failure cache;

[0070] The safe path verified by the trajectory optimization layer and the corresponding three-dimensional space-time safe interval are stored in the success cache;

[0071] The infeasible path fed back by the trajectory optimization layer and the corresponding three-dimensional space-time safe interval are stored in the failure cache;

[0072] If the current path matches a safe path in the success cache, and the three-dimensional safe space-time interval is completely consistent, the trajectory optimization layer directly reuses the trajectory stored in the success cache without recalculation;

[0073] If the current path matches an infeasible path recorded in the failure cache, and the three-dimensional space-time safe interval of the current path is a subset or completely the same as the three-dimensional space-time safe interval recorded in the failure cache, it is directly determined that the current path is infeasible, and the trajectory optimization layer does not continue to plan the current path;

[0074] The matching refers to having the same number of dynamic behavior primitives and distance distribution.

[0075] Beneficial effects:

[0076] (1) Three-layer planning: the behavior planning layer, the three-dimensional space-time constraint layer and the trajectory optimization layer are designed. The first two levels are responsible for global planning and collaborative coordination by the central controller, ensuring the overall cooperation and safety of the vehicle fleet; among them, the behavior planning layer generates a reference trajectory from a global perspective, and the three-dimensional space-time constraint layer generates a three-dimensional space-time safe interval based on the reference trajectory through spatial expansion. The trajectory optimization layer is autonomously executed by each unmanned vehicle, and under the restriction of the three-dimensional space-time safe interval, combined with the constraints of the vehicle itself, local path planning is performed to generate an optimal motion trajectory for real-time control and tracking. Not only does it improve real-time response capability and solution efficiency by sharing the computational burden, but it also generates various constraints under the restriction of the three-dimensional space-time safe interval to generate smooth and stable trajectories and optimal motion trajectories.

[0077] (2) In the behavior planning layer, the existing behavior primitive generation method is based on discrete space and fixed step time, and the generated trajectory is a point-to-point segmented connection, which may cause vibration or unstable motion in actual execution. Moreover, the discrete state space grows exponentially, reducing the computational efficiency. The present application extends the behavior primitive to the continuous time domain and generates dynamic behavior primitives in the continuous time domain, overcoming the discontinuity of the trajectory and the computational efficiency bottleneck caused by the discrete method, effectively solving the limitations of traditional discrete methods in smoothness and computational efficiency. It breaks through the limitations of discrete motion primitives, supports continuous time domain planning, and meets the real-time needs of high-density dynamic environments.

[0078] (3) In the core task of the three-dimensional space-time constraint layer, a safe three-dimensional space-time safety interval is defined for each vehicle according to the reference trajectory generated by the behavior planning layer. The existing area generation method mostly uses fixed geometry and pre-defined rules, which is difficult to adapt to dynamic environment. The fixed shape of the safety area limits the understanding space, and it is difficult to meet the dynamic demand. The application proposes a dynamic adjustment three-dimensional space-time constraint generation algorithm, which optimizes and adjusts the three-dimensional space-time safety interval according to the environment and vehicle state. The algorithm flexibly optimizes the shape and position of the area, efficiently allocates the motion space, reduces the conflict and redundant calculation, and improves the planning efficiency and adaptability in dynamic environment.

[0079] (4) Cache mechanism: a cache mechanism is designed between the three-dimensional space-time constraint layer and the trajectory optimization layer, which avoids repeated calculation by reusing successful areas and identifying failed areas in advance, and significantly improves the calculation efficiency in high-density scenarios.

[0080] (5) Hierarchical cooperation and optimization: through adaptive adjustment of centralized and distributed weights, the high-level centralized planning is responsible for low-frequency global path guidance; the low-level distributed planning is generated by each unmanned vehicle based on local perception in real time, which takes into account the real-time response and global coordination of the system. The heuristic weight adjustment rule maps the characteristics of complex scenes into weight parameters, reducing the computational complexity. The weight smooth switching mechanism decomposes and adjusts the small increment, and introduces the time dimension, avoiding the instability caused by jump, ensuring the smooth transition and robustness of the system. BRIEF DESCRIPTION OF DRAWINGS

[0081] Figure 1 The application provides a whole architecture diagram of an adaptive hierarchical multi-unmanned vehicle cooperative motion planning method under three-dimensional space-time constraints.

[0082] Figure 2 The behavior primitive expansion intention is provided for the embodiment of the application.

[0083] Figure 3 The three-dimensional space-time constraint interval is provided for the embodiment of the application.

[0084] Figure 4 The three-dimensional multi-vehicle motion planning trajectory is provided for the embodiment of the application. DETAILED DESCRIPTION

[0085] In order to more clearly, completely describe the purpose, technical scheme and advantages of the application, combined with the specific implementation process, further detailed description is made.

[0086] The application provides an adaptive hierarchical multi-unmanned vehicle cooperative motion planning method under three-dimensional space-time constraints, and a three-layer architecture cooperatively realizes efficient and safe cooperative planning and real-time response of multiple vehicles. As shown in Figure 1As shown, the scheme includes three levels, namely the behavior planning layer, the three-dimensional space-time constraint layer and the trajectory optimization layer; the behavior planning layer and the three-dimensional space-time constraint layer are arranged in the central controller, and are used for realizing global planning and collaborative coordination; and the trajectory optimization layer is arranged in each unmanned vehicle, and generates an optimal motion trajectory conforming to the actual dynamics constraint of the vehicle.

[0087] The behavior planning layer generates a dynamic behavior primitive based on the vehicle dynamics characteristics; and within the feasible domain limit of the global map information, an optimal dynamic behavior primitive sequence is screened to generate a globally optimal reference trajectory of each unmanned vehicle. This layer belongs to global planning, and can provide a long-time reference trajectory for a large range of scenes.

[0088] The three-dimensional space-time constraint layer generates a three-dimensional space-time safety interval of each unmanned vehicle according to the reference trajectory generated by the behavior planning layer and according to the environment and vehicle state; the three-dimensional space-time safety interval is not a large range, but is composed of drivable intervals corresponding to each time period on the trajectory. The purpose of the design of this layer is to form a multi-vehicle dynamic region around the reference trajectory to ensure interaction safety and interval rationality, and to provide a long-term safety feasible domain for each unmanned vehicle, so as to facilitate the limitation and guidance of the trajectory optimization layer arranged in each vehicle to perform local planning.

[0089] The trajectory optimization layer performs local planning on the reference trajectory based on local perception, under the constraint of the vehicle model and the three-dimensional space-time safety interval, and generates an optimal motion trajectory for control tracking in real time. This layer belongs to local planning, and calculates the final trajectory for control tracking.

[0090] The work of each layer will be described in detail below.

[0091] (1) Behavior planning layer

[0092] The method for the behavior planning layer to calculate a globally optimal multi-vehicle reference trajectory includes:

[0093] Step S11, generating a dynamic behavior primitive based on the vehicle dynamics characteristics.

[0094] To overcome the discontinuity of the segmented trajectory, the application introduces a dynamic behavior primitive (DBP) framework to extend the behavior primitive to the continuous time domain. In combination with the dynamics constraint, the generated trajectory is ensured to conform to the actual control requirements of the vehicle.

[0095] To extend the behavior primitive to the continuous time domain, the dynamic behavior primitive of the application uses a second-order dynamic system to model the evolution process of the trajectory, so that the generated trajectory is smooth. Specifically, the second-order state differential equation of the unmanned vehicle is combined with a nonlinear term to form an expression of the dynamic behavior primitive:

[0096]

[0097] Where y represents the autonomous vehicle's state (e.g., vehicle position), g represents the target state of the task, τ controls the trajectory speed, and α... y and β y Here, f(x) is the gain coefficient, and f(x) is a nonlinear term used to adjust the trajectory shape, represented by a weighted sum of basis functions, specifically:

[0098]

[0099] Where, ψ i (x)=exp(-h i (x-ci) 2 ) is the Gaussian function, ω i The weights are determined through trajectory learning; h i Let be the scaling parameter of the Gaussian function. Let be the variance, representing the range of the Gaussian distribution; y0 is the initial state; the phase variable x satisfies: α x Gain coefficient.

[0100] This dynamic model enables the generation of continuous, smooth trajectories, and allows for flexible adjustment of the trajectory target, speed, and duration through parameterization.

[0101] Equation (1) defines the dynamic behavior primitive. When generating the path, the task execution time is divided into N time segments, with each segment having a start and end time of t. s and t g For each segment n, a series of dynamic behavioral primitives are generated within the feasible domain constraints of the global map information according to formula (1), such as... Figure 2 As shown. If a collision occurs during path generation, the dynamic behavior primitive is discarded. During generation, given the initial state, a cluster of dynamic behavior primitives for the first segment is generated first. Then, for each dynamic behavior primitive, a cluster of dynamic behavior primitives for the second segment is generated, and so on, until the target is reached, ultimately forming a tree-like structure composed of dynamic behavior primitives. Based on this tree structure, multiple sequences of dynamic behavior primitives can be obtained from the root node to the leaf node through different paths.

[0102] The optimal dynamic behavior primitive sequence needs to be selected from multiple dynamic behavior primitive sequences.

[0103] Step S12: Design of the cost function for dynamic behavior primitives.

[0104] To select the optimal dynamic behavior primitive sequence, the behavior primitive cost function designed in this embodiment includes the node cost C. n (n), edge cost C e (epq ); where the node cost C n (n) is measured by the difference between node n in the trajectory and the goal point; the edge cost C e (e pq ) is measured by the trajectory smoothness, the task execution, the human-like driving features and the overall traffic efficiency of the trajectory e pq between node p and node q in the trajectory.

[0105] In a preferred embodiment, the node cost C n (n) and the edge cost C e (e pq ) are respectively represented as:

[0106] C n (n) = ω distance · dist(s n , s target ) + ω heading · Δθ(s n , s target ) (3)

[0107] C e (e pq ) = ω smooth · J P + ω task · C n (n q ) + ω human · C h (e pq ) + ω efficient · C v ||v e || (4)

[0108] In C n (n), n represents the nth node in the trajectory, s n is the current point state in the trajectory, s target is the goal point state in the trajectory; dist(s n , s target ) is the distance deviation, Δθ(s n , s target ) is the heading deviation, both of which are non-negative values; ω distance and ω heading represent the distance deviation weight and the heading deviation weight;

[0109] In C e (e pq ), e pq represents the edge between node n p and node n q in the trajectory; J P is the path smoothness; Cn (n q ) represents node n q The node cost; C h (e pq ) represents humanoid driving characteristics, based on node n q to node n p The angle deviation was obtained by looking up the human driving characteristic lookup table; C v ||v e || represents the overall traffic efficiency cost, C v With scalar ||v e ||negatively correlated,||v e The larger || is, the more C v The smaller, ||v e || represents edge e pq The corresponding average vehicle speed; ω smooth ω task ω human and ω efficient This indicates the corresponding weight, controlling the degree of influence of each cost.

[0110] Edge cost C e (e pq Path smoothness J in ) P This expresses the vehicle dynamics constraints introduced by the system, ensuring that the generated trajectory conforms to the actual control requirements of the vehicle. Path smoothness J P Represented as:

[0111]

[0112] Among them, t s and t g e pq The starting and ending points of the corresponding path; v(t) is the rear axle velocity of the autonomous vehicle, δ(t) is the front wheel steering angle, and L is the wheelbase; c s For steering control, ω z The yaw rate is used to minimize the overall trajectory curvature by minimizing the integral of the steering control quantity and the yaw rate.

[0113] Step S13: Generate the behavior primitive sequence.

[0114] With the objective of minimizing the overall objective function composed of node cost, edge cost, and path smoothness, a mixed-integer linear programming (MILP) model is used to jointly optimize the dynamic behavior primitive sequences of all autonomous vehicles, generating globally optimal reference trajectories for each vehicle. These reference trajectories conform to traffic rules (human-like driving characteristics), safety requirements, and vehicle dynamic characteristics.

[0115] The multi-vehicle behavior planning model is as follows:

[0116]

[0117] where N car the number of all unmanned vehicles, is the candidate edge cost of unmanned vehicle i, λ i is the weight of the corresponding unmanned vehicle, is the decision variable, if the edge e pq is selected for unmanned vehicle i otherwise

[0118] (2) Three-dimensional spatiotemporal constraint layer

[0119] The three-dimensional spatiotemporal constraint layer calculates the multi-vehicle dynamic region that is safe for interaction and reasonable in interval.

[0120] The three-dimensional spatiotemporal constraint layer resamples the reference path planned by the behavior planning layer into K time periods according to local computing resources, and the kth time period is denoted as time period

[0121] In order to improve the efficiency of generating multi-vehicle three-dimensional spatiotemporal intervals, two key performance indicators are constructed: the range width of the travel interval and the deviation degree from the reference trajectory

[0122]

[0123] where k represents the time period number, represents the range width weight of unmanned vehicle i; the area of the drivable interval in time period is represented by , see (c) in Figure 3 ; and is the maximum and minimum values of the drivable interval x and y directions; by maximizing the area of the planned drivable interval is ensured to be maximum. The need to be jointly optimized.

[0124] The three-dimensional spatiotemporal constraint layer integrates dynamic and static obstacle information, constructs a three-dimensional spatiotemporal map, segments the time axis, and defines the drivable interval of each vehicle within the time period. This region is no longer a simple fixed structure, but a more flexible boundary generated by a dynamic constraint model according to the kinematic characteristics and real-time state of the vehicle.

[0125] Figure 3 (b) in indicates the change of the three-dimensional spatiotemporal step occupying the time period. The deviation degree of the reference trajectory is specifically represented as:

[0126]

[0127] in, This refers to the deviation weight of driverless car i, with coordinates (x, y, y). i (k),y i (k) refers to the coordinates of a designated key point within the drivable section, preferably the centroid of the drivable section; coordinates For reference trajectory in time period The coordinates of the internal sampling time points (the sampling time points are the trajectory points in the trajectory); by minimizing Ensure that the planned drivable area closely matches the reference trajectory.

[0128] Obviously, the range of driving areas is wide It is a linear function, the deviation of the reference trajectory. It is a quadratic convex function. The cost of optimizing and integrating all autonomous vehicles in the three-dimensional spatiotemporal interval is ultimately expressed as:

[0129]

[0130] Where, η i Let be the weight of autonomous vehicle i, and represent its priority. For the optimization problem of minimizing the objective function, since the goal of generating the three-dimensional spatiotemporal interval is to maximize the driving interval, therefore... It has a minus sign in front of it.

[0131] With J corridor With the goal of minimizing costs, we will jointly optimize all driverless vehicles and all time periods. The drivable area is determined, and the three-dimensional spatiotemporal safety zone of all autonomous vehicles is obtained. To avoid collisions when multiple autonomous vehicles interact, the areas represented in the three-dimensional spatiotemporal map cannot overlap. During optimization, it is also necessary to restrict multiple vehicles from occupying the same spatial position at the same time.

[0132] The resampled reference trajectory, along with the three-dimensional spatiotemporal safety zone, is sent to the trajectory optimization layer of the corresponding unmanned vehicle.

[0133] In one preferred scheme, to improve computational efficiency in high-density scenarios, a caching mechanism is introduced between the trajectory optimization layer and the three-dimensional spatiotemporal constraint layer, including success caching and failure caching.

[0134] The success cache stores verified safe paths and their corresponding three-dimensional spatiotemporal safe intervals, while the failure cache records infeasible paths and their corresponding three-dimensional spatiotemporal safe intervals to infer infeasibility.

[0135] If the current path is "pseudo-identical" to a safe path in the success cache, i.e., has the same number of dynamic behavior primitives and similar distance distribution (if two paths P1 and P2 satisfy the point number is the same, for each pair of adjacent points (p i ,p i+1 )∈P1, (q i ,q i+1 )∈P2 distance d(p i ,p i+1 )≈d(q i ,q i+1 ), it is considered that the two paths have "similar distance distribution", and their three-dimensional space-time safety intervals are completely identical, then the trajectory optimization layer can directly reuse the optimal motion trajectory stored in the success cache, skipping the recalculation process.

[0136] If the current path is "pseudo-identical" to the infeasible path recorded in the failure cache, and its three-dimensional space-time safety interval is a subset or completely identical to the three-dimensional space-time safety interval recorded in the failure cache, then the current path can be directly determined as infeasible, and the trajectory optimization layer does not continue to plan the current path, avoiding redundant calculation.

[0137] The above "pseudo-identical" judgment can be performed after the three-dimensional space-time constraint layer determines the three-dimensional space-time safety interval, or after the trajectory optimization layer receives the three-dimensional space-time safety interval data. The former advantage is that only global data needs to be maintained in the three-dimensional space-time constraint layer, and the latter advantage is that the "pseudo-identical" judgment can be personalized.

[0138] The region boundary dynamically adjusted by the three-dimensional space-time constraint layer is combined with the cache mechanism, effectively improving the calculation efficiency and adaptability of path planning, and providing stable and efficient space-time constraints for the trajectory optimization layer.

[0139] (3) Trajectory optimization layer

[0140] The trajectory optimization layer performs local planning of the path based on the reference path. According to the initial state information of the vehicle, the control quantity is optimized, the longitudinal and lateral coupling vehicle model constraints are considered, the trajectory smoothness and comfort are optimized, and finally a safe trajectory suitable for vehicle tracking control is generated.

[0141] Step S31: First, construct global path feasibility constraints and local path deviation rate constraints; maximize global path feasibility to ensure that the planned path meets vehicle capability and ride comfort; minimize local path deviation rate to ensure that the planned path is close to the reference trajectory issued by the three-dimensional space-time constraint layer.

[0142] ① Global path feasibility constraints include trajectory smoothness constraints and trajectory comfort constraints

[0143] Trajectory smoothness constraint Related to path curvature:

[0144]

[0145] where t represents the sampling time point of the trajectory optimization layer, the time interval Δt is less than the unit time period interval t(k)-t(k-1) of the spatio-temporal step in the three-dimensional spatio-temporal constraint layer, represents the trajectory smoothness weight of the unmanned vehicle i. The scheme is characterized by the integral of the square term of the path curvature , the smaller the curvature, the better the smoothness performance, and the curvature is converted into the front wheel steering angle . is the front wheel steering angle, and L is the wheelbase of the vehicle.

[0146] Trajectory comfort constraint Involves two aspects of path and speed:

[0147]

[0148] wherein, represents the front wheel steering angle change rate weight of the unmanned vehicle i, is the acceleration change rate weight. Reflects the ride comfort, and is negatively correlated with the amplitude value of the front wheel steering angle change rate and the acceleration change rate .

[0149] ②Local path deviation rate constraint is an interval boundary constraint, which uses the deviation degree of the key point, which is calculated in the three-dimensional spatio-temporal configuration space.

[0150]

[0151] wherein, and respectively represent the deviation degree weight of the unmanned vehicle i in the x-axis and y-axis directions from the key point; represents the node coordinates obtained by the trajectory optimization layer for local path planning in the time period in the reference trajectory issued by the three-dimensional spatio-temporal constraint layer, which is the object to be optimized; (x i (k), y i (k)) represents the key point coordinates corresponding to the drivable interval; since the sampling time interval of the trajectory optimization layer is less than the unit time period length of the three-dimensional spatio-temporal constraint layer, the trajectory point coordinates in each time period k Regarding the key point coordinates (xi (k),y i (k)) are time-matched. The local path deviation rate The planned path is ensured to be close to the reference trajectory issued by the three-dimensional space-time constraint layer. Here, the key point refers to the specified key point in the drivable interval, such as the centroid.

[0152] Step S32: Adaptive dynamic weight calculation.

[0153] The trajectory optimization layer evaluates the global path feasibility and the local path deviation rate, and dynamically adjusts the weights of the centralized and distributed systems according to the current environmental complexity and the current communication bandwidth to adapt to different scene requirements. The weight distribution formula is:

[0154] Global path feasibility constraint weight

[0155] Local path deviation rate constraint weight W distributed = 1-W centralized (13)

[0156] where C env is the complexity of the current environment, C max is the maximum threshold of environmental complexity; B comm represents the current communication bandwidth, and B max is the maximum communication bandwidth.

[0157] To ensure the smoothness of weight adjustment and the stability of system operation, the present application introduces a smooth switching mechanism to gradually transition the centralized and distributed weights, avoiding the influence of weight changes on trajectory stability and vehicle motion control. The smooth switching mechanism decomposes the weight adjustment process into small incremental changes by introducing a time dimension, gradually approaching the target weight value. Exponential smoothing adjusts faster in the early stage and gradually converges in the later stage, which is suitable for scenes that are more sensitive to weight changes. Update W distributed at each sampling time t, and the update formula is:

[0158] W centralized (t) = W centralized (t-1) + ΔW·e -λt (14)

[0159] where W centralized (t) is the updated weight value at time t, W centralized (t-1) is the weight value used at time t-1; ΔW = W target - W centralized (t-1) is the difference between the weight value at time t-1 and the weight target value W target calculated based on environmental complexity and communication bandwidth; λ is the convergence rate coefficient.

[0160] The trajectory optimization generation algorithm of each unmanned vehicle integrates the cost of formula (10), formula (11) and formula (12) as a final target function:

[0161] Step S33: The trajectory optimization generation algorithm of each unmanned vehicle integrates the cost of formula (10), formula (11) and formula (12) as a final target function

[0162]

[0163] The trajectory optimization layer under the three-dimensional space-time map takes formula (15) as a total target function, takes the minimization of the total target function as a goal, takes the longitudinal and lateral coupled vehicle kinematics model and the three-dimensional space-time corridor boundary as a hard constraint, constructs an unmanned vehicle trajectory optimization model considering the vehicle model, processes the nonlinear optimization problem, realizes joint optimization of each time period in the reference trajectory issued by the three-dimensional space-time constraint layer, and obtains an optimal motion trajectory.

[0164] In summary, the application proposes a self-adaptive hierarchical multi-vehicle cooperative motion planning method based on three-dimensional space-time constraints. Through multi-level cooperative work, the centralized and distributed weights and the smooth switching mechanism are adaptively and dynamically adjusted to ensure safety, real-time performance and cooperative efficiency in complex dynamic environments. The application generates behavior primitives in the continuous time domain, effectively solving the limitations of traditional discrete methods in smoothness and computational efficiency. In the space-time constraint layer, the space-time safety region is dynamically adjusted, and in the trajectory optimization layer, the vehicle dynamics characteristics and path smoothness are considered to ensure that the generated trajectory meets the safety requirements and satisfies the motion characteristics of the vehicle.

[0165] The above specific embodiments only describe the design principles of the application, and the shapes and names of the components in the description can be different and are not limited. Therefore, the skilled in the art can modify or equivalently replace the technical solutions described in the foregoing embodiments; and these modifications and replacements do not deviate from the purpose and technical solution of the application, and should all belong to the protection scope of the application.

Claims

1. A method for adaptive hierarchical multi-unmanned vehicle cooperative motion planning under three-dimensional space-time constraints, characterized in that, Comprise: The behavior planning layer, the three-dimensional space-time constraint layer and the trajectory optimization layer are constructed; the behavior planning layer and the three-dimensional space-time constraint layer are arranged in the central controller, and are used for realizing global planning and collaborative coordination; the trajectory optimization layer is arranged in each unmanned vehicle, and generates an optimal motion trajectory conforming to a dynamic constraint; The behavior planning layer generates a dynamic behavior primitive based on vehicle dynamics characteristics; In the feasible domain limit of the global map information, an optimal dynamic behavior primitive sequence is screened, and a globally optimal reference trajectory of each unmanned vehicle is generated; wherein, the manner of screening the primitive sequence and generating the reference trajectory is: designing a behavior primitive cost function includes a node cost and an edge cost ; Node cost By the difference between the node in the trajectory n And the target point Edge cost From the nodes in the trajectory p and nodes q Trajectory between It is measured by trajectory smoothness, task execution, human-like driving characteristics, and overall traffic efficiency; Mixed Integer Linear Programming (MILP) is used, based on node cost. and edge cost By jointly optimizing the dynamic behavior primitive sequences of each autonomous vehicle, a globally optimal reference trajectory for each autonomous vehicle is formed. The three-dimensional space-time constraint layer generates a three-dimensional space-time safety interval of each unmanned vehicle according to the reference trajectory generated by the behavior planning layer, in combination with the environment and the vehicle state; the three-dimensional space-time safety interval is composed of drivable intervals corresponding to each time period on the trajectory; when the three-dimensional space-time safety interval is generated, the range of the driving interval is wide, the degree of deviation of the key point reference trajectory of the driving interval is small, and the generated target is limited to not interfering with the three-dimensional space-time environment and the drivable intervals of multiple vehicles not occupying the same space position at the same time period; Wherein, the manner of generating the three-dimensional space-time safety interval is: a reference trajectory planned by the behavior planning layer is resampled according to local computing resources into K a time period, the k time period is denoted as time period ; Constructing key performance indicators, including range of travel section and degree of deviation from reference trajectory ; The range of the travel section is wide Is: wherein, denotes the time period index; denotes the range width weight of the unmanned vehicle ; the drivable range area of the time period is characterized by ; , , , is the maximum and minimum value of the drivable range x , y direction; by maximizing , the total planned drivable range area is ensured to be overall maximum; The degree of deviation from the reference trajectory is: wherein, is a deviation degree weight of the unmanned vehicle , coordinates is a time period corresponding to the specified key point within the drivable range, coordinates is a time period , coordinates of the reference trajectory sampling time point within the time period; by minimizing , it is ensured that the planned drivable range is close to the reference trajectory; The optimization of the three-dimensional space-time safety interval integrates the cost of all unmanned vehicles, and the final objective function is represented as: wherein, is the weight of the unmanned vehicle represents the priority; To minimize the total travel time of all unmanned vehicles while limiting the interference with the three-dimensional space-time environment and the occupation of the same space position by multiple vehicles at the same time period, jointly optimize the feasible intervals of all unmanned vehicles and all time periods to obtain the three-dimensional space-time safety intervals of all unmanned vehicles; The resampled reference trajectory is sent to the trajectory optimization layer of the corresponding unmanned vehicle together with the three-dimensional space-time safety interval; The trajectory optimization layer implements local planning on the reference trajectory based on local perception under the constraint of the vehicle model and the three-dimensional space-time safety interval, and generates an optimal motion trajectory for control tracking in real time; Wherein, the manner of implementing local planning on the reference trajectory is: Global path feasibility constraints and local path deviation rate constraints are constructed; the global path feasibility is maximized to ensure that the planned path meets the vehicle capability and ride comfort; the local path deviation rate is minimized to ensure that the planned path is close to the reference trajectory issued by the three-dimensional space-time constraint layer; According to the current environment complexity and the current communication bandwidth, the weights of the global path feasibility constraint and the local path deviation rate constraint are dynamically adjusted; Using the weights, the global path feasibility constraint and the local path deviation rate constraint are weighted to obtain a total objective function of the optimal motion trajectory of the unmanned vehicle; The total objective function of the optimal motion trajectory is minimized as the target, and each time period in the reference trajectory issued by the three-dimensional space-time constraint layer is jointly optimized to obtain the optimal motion trajectory.

2. The method of claim 1, wherein, In the behavior planning layer, the dynamic behavior primitive is generated based on vehicle dynamics characteristics: The second-order state differential equation of the unmanned vehicle is combined with the nonlinear term to form an expression of the dynamic behavior primitive: wherein, is the state of the unmanned vehicle, is the target state of the mission, controls the trajectory velocity, and is a gain coefficient, is a nonlinear term for adjusting the shape of the trajectory, represented by a weighted sum of basis functions.

3. The method of claim 1, wherein, The node cost is: wherein, n represents the i-th node in the trajectory, n is the state of the node n in the trajectory, is the target point state in the trajectory; is the distance deviation, is the heading deviation, both being non-negative values; and represent the distance deviation weight and the heading deviation weight, respectively.​ the side cost is: wherein, denotes an edge between nodes and nodes ; is a path smoothness; denotes a node cost of node ; denotes a human-like driving feature, obtained from a human-like driving feature lookup table according to an angle deviation of node to node ; denotes an overall traffic efficiency cost, negatively correlated with a scalar ; denotes an average speed corresponding to edge ; , , and denote weights of the corresponding items; edge cost path smoothness in is expressed as: wherein, and are respectively the start and end points of the corresponding path; is the rear axle speed of the autonomous vehicle, is the front wheel angle of the vehicle, is the wheel base of the vehicle; is the steering control amount, is the yaw rate, by minimizing the integral of the product of the steering control amount and the yaw rate, the overall curvature of the trajectory is ensured to be minimal; In the feasible domain limit of the global map information, a total objective function composed of node cost, edge cost and path smoothness is minimized as the target, and a MILP model is used to jointly optimize the dynamic behavior primitive sequence of all unmanned vehicles to generate globally optimal reference trajectories of each unmanned vehicle.

4. The method of claim 1, wherein, The global path feasibility constraints include trajectory smoothness constraints and trajectory comfort constraints ; The trajectory smoothness constraint is: wherein, denotes a sampling time point of the trajectory optimization layer, denotes an unmanned vehicle a trajectory smoothness weight; is a front wheel steering angle, is a vehicle wheelbase; The trajectory comfort constraint Is: in, Indicates driverless car Front wheel steering angle change rate The weight, For driverless cars rate of change of acceleration The weights; The local path deviation rate constraint is: wherein, represents the time period in the reference trajectory of the unmanned vehicle issued by the trajectory optimization layer under the three-dimensional space-time constraint layer The node coordinates obtained by performing local path planning are objects that need to be optimized. represents The key point coordinates of the drivable interval to which the unmanned vehicle belongs. and respectively represent the deviation degree weight of the unmanned vehicle from the key point in the axis and axis direction; by minimizing the local path deviation rate , the planned local path is close to the reference trajectory within the drivable interval.

5. The method of claim 1, wherein, weight of the global path feasibility constraint weight of the local path deviation rate constraint respectively: wherein, is a complexity of the current environment, is a maximum threshold for the complexity of the environment; denotes a current communication bandwidth, is a maximum communication bandwidth.

6. The method of claim 5, wherein, At each sampling time t, update The update formula is wherein, is the updated weight value at time t, is the weight value used at time t; is the difference between the weight value at time t and a weight target value calculated based on the environmental complexity and the communication bandwidth; is a convergence rate coefficient.​ 7. The method of claim 5, wherein, A cache mechanism is further arranged between the three-dimensional space-time constraint layer and the trajectory optimization layer, including successful cache and failed cache; In the successful cache, the safe path verified by the trajectory optimization layer and the corresponding three-dimensional space-time safety interval are stored. The infeasible path and corresponding three-dimensional time-space safety interval stored in the failure cache store trajectory optimization layer feedback; If the current path matches a safe path in the success cache, and the three-dimensional safety time-space interval is completely consistent, the trajectory optimization layer directly reuses the trajectory stored in the success cache without recalculation; If the current path matches the infeasible path recorded in the failure cache, and the three-dimensional time-space safety interval of the current path is a subset or completely the same as the three-dimensional time-space safety interval recorded in the failure cache, it is directly judged that the current path is infeasible, and the trajectory optimization layer does not continue to plan the current path; The matching refers to having the same number of dynamic behavior primitives and distance distribution.

Images