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

Abstract

The invention provides a self-adaptive hierarchical multi-unmanned vehicle cooperative motion planning method under three-dimensional space-time constraint. The method comprises the following steps: constructing 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 on the central controller, and 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, and provides a suitable space range which is as large as possible and is close to the reference trajectory for trajectory optimization of the trajectory optimization layer; and the trajectory optimization layer is arranged on each unmanned vehicle, carries out local planning on a reference trajectory based on local perception under the constraints of a vehicle model and a three-dimensional space-time safety interval, and generates an optimal motion trajectory for controlling tracking in real time. According to the method, the motorcade cooperation efficiency and safety in a complex dynamic environment can be remarkably improved.

Description

Technical Field

[0001] The invention belongs to the technical field of robots and relates to an adaptive hierarchical multi-unmanned vehicle collaborative motion planning method under three-dimensional space-time constraints. Background Art

[0002] With the rapid development of unmanned driving technology, especially in the fields of intelligent transportation, autonomous driving fleets, logistics and disaster relief, the coordinated motion planning of multiple unmanned vehicles has become an important research direction. The coordinated motion planning of multiple vehicles requires not only precise control of the motion of a single vehicle, but also efficient collaboration and coordination between vehicles. How to improve overall efficiency while ensuring safety has become the core challenge of current technical research.

[0003] At present, multi-vehicle collaborative motion planning is mainly divided into two architectures: centralized and distributed. The centralized method relies on the central controller to provide the global optimal solution, but the computational burden is heavy and it is difficult to adapt to large-scale and high-real-time scenarios. The distributed method improves scalability and response speed through decentralized computing, but lacks global coordination and is prone to falling into local optimality. Although the hierarchical algorithm optimizes both, it generates multiple branches when dealing with conflicts, each branch solves the conflicting path separately, has many node extensions, high computation time, and insufficient adaptability.

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

[0005] In terms of path and speed optimization, existing methods often separate path and speed planning and lack a coordination mechanism, which leads to mismatch between trajectory and speed, affecting vehicle stability and control accuracy. The simplified planning model does not fully consider dynamic constraints, resulting in infeasible planning results.

[0006] Although some progress has been made, existing technologies still face challenges such as dynamic obstacle avoidance and global and local optimization balance. Traditional single-vehicle planning methods cannot meet the needs of multi-vehicle collaboration, especially in rapidly changing environments. Therefore, there is an urgent need for an efficient and universal collaborative planning method that can effectively coordinate behavioral decision-making and trajectory optimization in complex scenarios to improve collaborative efficiency, safety, and adaptability. Summary of the invention

[0007] In view of this, the present invention provides an adaptive hierarchical multi-unmanned vehicle collaborative motion planning method under three-dimensional space-time constraints, which can significantly improve the collaborative efficiency and safety of the fleet in complex dynamic environments.

[0008] In order to solve the above technical problems, the present invention is implemented as follows.

[0009] An adaptive hierarchical multi-unmanned vehicle collaborative motion planning method under three-dimensional space-time constraints, comprising:

[0010] Construct 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 set in the central controller to achieve global planning and collaborative coordination; the trajectory optimization layer is set in each unmanned vehicle to generate the optimal motion trajectory that meets the dynamic constraints;

[0011] The behavior planning layer generates dynamic behavior primitives based on vehicle dynamics characteristics. It selects the optimal dynamic behavior primitive sequence within the feasible domain of global map information and generates the globally optimal reference trajectory of each unmanned vehicle.

[0012] The three-dimensional space-time constraint layer generates the three-dimensional space-time safety interval of each unmanned vehicle based on the reference trajectory generated by the behavior planning layer, combined with the environment and vehicle status. The three-dimensional space-time safety interval is composed of the drivable interval corresponding to each time period on the trajectory; when generating the three-dimensional space-time safety interval, the generation goal is to have a large range of driving intervals and a small deviation from the reference trajectory of key points in the driving interval. At the same time, it is restricted not to interfere with the three-dimensional space-time environment and the drivable intervals of multiple vehicles are not allowed to occupy the same spatial position in the same time period;

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

[0014] Preferably, in the behavior planning layer, based on the vehicle dynamics characteristics, the dynamic behavior primitives are generated as follows:

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

[0016]

[0017] Among them, y is the state of the unmanned vehicle, g is the target state of the task, τ controls the trajectory speed, and α y and β y is the gain coefficient, f(x) is the nonlinear term used to adjust the trajectory shape, and is represented by the weighted sum of basis functions.

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

[0019] Design behavior primitive cost function including node cost C n (n) and edge cost C e (e pq );

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

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

[0022] Using mixed integer linear programming MILP, based on the node cost C n (n) and edge cost C e (e pq ) Jointly optimize the dynamic behavior primitive sequences of each unmanned vehicle 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] Where 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 values; ω distance and ω heading Indicates the distance deviation weight and heading deviation weight;

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

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

[0028] Among them, e pq Represents node n in the trajectory p and node n q The edge between P is the path smoothness; C n (n q ) represents node n q The node cost of C h (e pq ) represents human-like driving characteristics, according to node n q To node n p The angle deviation is obtained by querying the human-like driving feature lookup table; C v ||v e || represents the overall traffic efficiency cost, and the scalar ||v e || is negatively correlated; ||v e || represents edge e pq The corresponding average vehicle speed; ω smooth ,ω task ,ω human and ω efficient represents the weight of the corresponding item;

[0029] Edge cost C e (e pq ) in the path smoothness J P It is expressed as:

[0030]

[0031] Among them, t s and t g e pq The starting point and end point of the corresponding path; v(t) is the rear axle speed of the unmanned vehicle, δ(t) is the front wheel turning angle of the vehicle, and L is the wheelbase of the vehicle; c s is the steering control quantity, ω z is the yaw rate, which ensures the minimum overall curvature of the trajectory by minimizing the integral of the steering control amount and the yaw rate;

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

[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 recorded as the time period

[0035] Build key performance indicators, including driving range breadth and the deviation from the reference trajectory

[0036] The breadth of the driving range for:

[0037]

[0038] Where k represents the time period number; represents the range breadth weight of unmanned vehicle i; time period The drivable area is given by to characterize; is the maximum and minimum value of the x and y directions of the drivable range; by maximizing Ensure that the planned drivable area is the largest overall;

[0039] The deviation from the reference trajectory for:

[0040]

[0041] in, Refers to the deviation weight of unmanned vehicle i, coordinate (x i (k), y i (k)) refers to the time period The coordinates of the specified key points in the corresponding drivable range, For time period The coordinates of the sampling time points of the internal reference trajectory; by minimizing Ensure that the planned drivable range is close to the reference trajectory;

[0042] The optimization of the three-dimensional space-time safety interval generates the cost of integrating all unmanned vehicles, and the final objective function is expressed as:

[0043]

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

[0045] J corridor The goal is to minimize the number of unmanned vehicles and to limit the interference with the three-dimensional space-time environment. Multiple vehicles are not allowed to occupy the same space in the same time period. All unmanned vehicles and all time periods are jointly optimized. The drivable range is obtained to 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, local planning is performed on the reference trajectory as follows:

[0048] Construct global path feasibility constraints and local path deviation rate constraints; maximize global path feasibility to ensure that the planned path meets vehicle capabilities 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;

[0049] Dynamically adjust the weight of the global path feasibility constraint and the weight of the local path deviation rate constraint according to the current environment complexity and the current communication bandwidth;

[0050] Using weights, the global path feasibility constraint and the local path deviation rate constraint are weighted to obtain the overall objective function of the optimal motion trajectory of the unmanned vehicle;

[0051] With the goal of minimizing the total objective function of the optimal motion trajectory, 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.

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

[0053] The trajectory smoothness constraint for:

[0054]

[0055] Among them, t represents the sampling time point of the trajectory optimization layer, represents the trajectory smoothness weight of unmanned vehicle i; is the front wheel turning angle, L is the vehicle wheelbase;

[0056] The trajectory comfort constraint for:

[0057]

[0058] in, represents the rate of change of the front wheel turning angle of the unmanned vehicle i The weight of is the acceleration change rate of unmanned vehicle i The weight of

[0059] The local path deviation rate constraint for:

[0060]

[0061] in, Represents the time period in the reference trajectory of the unmanned vehicle i issued by the trajectory optimization layer to the three-dimensional space-time constraint layer The node coordinates obtained by local path planning are the objects that need to be optimized; (x i (k),y i (k)) indicates Coordinates of key points in the drivable area; and Respectively represent the deviation weights of the unmanned vehicle i from the key points in the x-axis and y-axis directions; by minimizing the local path deviation rate Make the planned local path close to the reference trajectory within the drivable range.

[0062] Preferably, the weight W of the global path feasibility constraint is centralized The weight W of the local path deviation rate constraint distributed They are:

[0063]

[0064] W distributed =1-W centralized

[0065] Among them, C env is the complexity of the current environment, C max is the maximum threshold of environmental complexity; B comm Indicates the current communication bandwidth, B max is the maximum communication bandwidth.

[0066] Preferably, W distributed Changes over time, and updates W at each sampling time t distributed , the update formula is:

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

[0068] Among them, 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 weight value at time t-1 and the weight target value W calculated based on the environment complexity and communication bandwidth target The difference between them; λ is the convergence rate coefficient.

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

[0070] The successful cache stores the verified safe paths and corresponding three-dimensional space-time safe intervals of the trajectory optimization layer;

[0071] The infeasible paths fed back by the trajectory optimization layer and the corresponding three-dimensional space-time safety intervals are stored in the failure buffer;

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

[0073] If the current path matches the infeasible path recorded in the failure cache, and the 3D space-time safety interval of the current path is a subset of or exactly the same as the 3D space-time safety interval recorded in the failure cache, then the current path is directly judged to be 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 present invention designs a behavior planning layer, a three-dimensional space-time constraint layer, and a trajectory optimization layer. The central controller is responsible for global planning and coordination in the first two layers to ensure the overall collaboration and safety of the 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 safety interval through spatial expansion based on the reference trajectory. The trajectory optimization layer is autonomously executed by each unmanned vehicle. Under the constraints of the three-dimensional space-time safety interval, combined with the constraints of the vehicle itself, local path planning is performed to generate the optimal motion trajectory for control tracking in real time. Not only does it improve the real-time response capability and solution efficiency by sharing the computational burden, but it can also generate various constraints under the constraints of the three-dimensional space-time safety interval to generate smooth and stable trajectories and optimal motion trajectories.

[0077] (2) At 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 during actual execution. Moreover, the discrete state space grows exponentially, and the computational efficiency is reduced. In response to these limitations, the present invention extends the behavior primitives to the continuous time domain and generates dynamic behavior primitives in the continuous time domain, overcoming the trajectory discontinuity and computational efficiency bottleneck caused by the discrete method, and effectively solving the limitations of the traditional discrete method in smoothness and computational efficiency. It breaks through the limitations of discrete motion primitives, supports continuous time domain planning, and meets the real-time requirements of high-density dynamic environments.

[0078] (3) In the three-dimensional space-time constraint layer, the core task is to define a safe three-dimensional space-time safety zone for each vehicle based on the reference trajectory generated by the behavior planning layer. Existing area generation methods mostly use fixed geometric shapes and predefined rules, which are difficult to adapt to dynamic environments. The fixed-shape safety area limits the understanding space and is difficult to meet dynamic needs. The present invention proposes a dynamically adjusted three-dimensional space-time constraint generation algorithm to optimize and adjust the three-dimensional space-time safety zone according to the environment and vehicle status. The algorithm flexibly optimizes the shape and position of the area, efficiently allocates the motion space, reduces conflicts and redundant calculations, and improves the planning efficiency and adaptability in dynamic environments.

[0079] (4) Cache mechanism: A cache mechanism is designed between the 3D spatiotemporal constraint layer and the trajectory optimization layer. By reusing successful areas and identifying failed areas in advance, repeated calculations can be avoided, significantly improving the computing efficiency in high-density scenarios.

[0080] (5) Hierarchical collaboration and optimization: Through adaptive adjustment of centralized and distributed weights, high-level centralized planning is responsible for low-frequency global path guidance; low-level distributed planning is performed by each unmanned vehicle based on local perception to generate collision-free trajectories in real time, taking into account the real-time response and global coordination of the system. The heuristic weight adjustment rule maps complex scene features into weight parameters to reduce computational complexity. The weight smoothing switching mechanism is decomposed and adjusted into small increments, and the time dimension is introduced to avoid instability caused by jumps, ensuring smooth transition and robustness of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 This is an overall architecture diagram of an adaptive hierarchical multi-unmanned vehicle collaborative motion planning method under three-dimensional space-time constraints provided by the present invention.

[0082] Figure 2 A schematic diagram of behavior primitive expansion provided by an embodiment of the present invention.

[0083] Figure 3 A schematic diagram of a three-dimensional space-time constraint interval provided in an embodiment of the present invention.

[0084] Figure 4 A spatiotemporal three-dimensional multi-vehicle motion planning trajectory provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0085] In order to more clearly and completely describe the objectives, technical solutions and advantages of this application, further detailed description is now provided in combination with the specific implementation process.

[0086] The present invention provides an adaptive hierarchical multi-unmanned vehicle collaborative motion planning method under three-dimensional space-time constraints, and the three-layer architecture collaboratively realizes efficient and safe collaborative planning and real-time response of multiple vehicles. Figure 1As shown in the figure, the scheme includes three levels, namely, behavior planning layer, three-dimensional space-time constraint layer and trajectory optimization layer; the behavior planning layer and the three-dimensional space-time constraint layer are set in the central controller to achieve global planning and collaborative coordination; the trajectory optimization layer is set in each unmanned vehicle to generate the optimal motion trajectory that meets the dynamic constraints of the actual vehicle.

[0087] The behavior planning layer generates dynamic behavior primitives based on vehicle dynamics characteristics; within the feasible domain of global map information, it selects the optimal dynamic behavior primitive sequence and generates the globally optimal reference trajectory of each unmanned vehicle. This layer belongs to global planning and can provide long-term reference trajectories for a wide range of scenarios.

[0088] The three-dimensional space-time constraint layer generates the three-dimensional space-time safety interval of each unmanned vehicle based on the reference trajectory generated by the behavior planning layer, the environment and the vehicle status; the three-dimensional space-time safety interval is not a large range, but is composed of the drivable intervals corresponding to each time period on the trajectory. The purpose of this layer design is to form a multi-vehicle dynamic area around the reference trajectory to ensure interactive safety and interval rationality, providing a long-term safe and feasible domain for each unmanned vehicle, and facilitating the restriction and guidance of the trajectory optimization layer set on each vehicle for local planning.

[0089] The trajectory optimization layer implements local planning for the reference trajectory based on local perception, under the constraints of the vehicle model and the three-dimensional space-time safety interval, and generates the 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 working of each layer is described in detail below.

[0091] (1) Behavior Planning Layer

[0092] The method for calculating the global optimal multi-vehicle reference trajectory in the behavior planning layer includes:

[0093] Step S11, generating dynamic behavior primitives based on vehicle dynamics characteristics.

[0094] In order to overcome the piecewise discontinuity of discrete trajectories, the present invention introduces the Dynamic Behavioral Primitives (DBP) framework, which extends the behavioral primitives to the continuous time domain and combines dynamic constraints to ensure that the generated trajectory meets the actual control requirements of the vehicle.

[0095] In order to extend the behavior primitives to the continuous time domain, the dynamic behavior primitives of the present invention use the second-order dynamic system to model the evolution of the trajectory, so that the generated trajectory is smooth. Specifically, the second-order state differential equation of the unmanned vehicle is combined with the nonlinear term to form the expression of the dynamic behavior primitive:

[0096]

[0097] Among them, y is the state of the unmanned vehicle (such as the vehicle position), g is the target state of the task, τ controls the trajectory speed, and α y and β y is the gain coefficient, f(x) is the nonlinear term used to adjust the trajectory shape, which is represented by the weighted sum of basis functions, specifically:

[0098]

[0099] Among them, ψ i (x) = exp(-h i (x-ci) 2 ) is the Gaussian basis function, ω i is the weight, determined by trajectory learning; h i is the scale parameter of the Gaussian function, is the variance, indicating the extension range of the Gaussian distribution; y 0 is the initial state; the phase variable x satisfies: α x Gain factor.

[0100] Through this dynamic model, continuous and smooth trajectories can be generated, and the trajectory target, speed and duration can be flexibly adjusted through parameterization.

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

[0102] It is necessary to select the optimal dynamic behavior primitive sequence from multiple dynamic behavior primitive sequences.

[0103] Step S12, dynamic behavior primitive cost function design.

[0104] In order 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 Ce (e pq ), where the node cost C n (n) is measured by the difference between node n and the target point in the trajectory; the edge cost C e (e pq ) is the trajectory e between node p and node q in the trajectory pq The vehicle’s driving performance is measured by trajectory smoothness, task execution, human-like driving characteristics, and overall traffic efficiency.

[0105] In a preferred embodiment, the node cost C n (n) and edge cost C e (e pq ) are respectively expressed 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] C n In (n), n represents the nth node in the trajectory, s n is the current point state 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 values; ω distance and ω heading Indicates the distance deviation weight and heading deviation weight;

[0109] C e (e pq ), e pq Represents node n in the trajectory p and node n q The edge betweenP is the path smoothness; C n (n q ) represents node n q The node cost of C h (e pq ) represents human-like driving characteristics, according to node n q To node n p The angle deviation is obtained by looking up the human-like driving feature lookup table; C v ||v e || represents the overall traffic efficiency cost, C v With scalar ||v e || is negatively correlated, ||v e ||The larger the C v The smaller ||v e || represents edge e pq The corresponding average vehicle speed; ω smooth ,ω task ,ω human and ω efficient Represents the corresponding weight, controlling the influence of each cost.

[0110] Edge cost C e (e pq ) in the path smoothness J P It expresses the vehicle dynamics constraints introduced by the system to ensure that the generated trajectory meets the actual control requirements of the vehicle. P It is expressed as:

[0111]

[0112] Among them, t s and t g e pq The starting point and end point of the corresponding path; v(t) is the rear axle speed of the unmanned vehicle, δ(t) is the front wheel turning angle of the vehicle, and L is the wheelbase of the vehicle; c s is the steering control quantity, ω z The yaw rate is the total curvature of the trajectory, which is minimized by minimizing the integral of the steering control amount and the yaw rate.

[0113] Step S13, generating a behavior primitive sequence.

[0114] With the goal of minimizing the total objective function consisting of node cost, edge cost, and path smoothness, the mixed integer linear programming (MILP) model is used to jointly optimize the dynamic behavior primitive sequences of all unmanned vehicles to generate the global optimal reference trajectory of each unmanned vehicle. The reference trajectory meets traffic rules (human-like driving characteristics), safety requirements, and vehicle dynamic characteristics.

[0115] The model used for multi-vehicle behavior planning is:

[0116]

[0117] Among them, N car The number of all driverless cars, is the candidate edge cost of unmanned vehicle i, λ i is the weight corresponding to the unmanned vehicle, As a decision variable, if we choose the edge e of unmanned vehicle i pq but otherwise

[0118] (2) Three-dimensional space-time constraint layer

[0119] The three-dimensional space-time constraint layer calculates the multi-vehicle dynamic area with interactive safety and reasonable interval.

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

[0121] In order to improve the efficiency of generating multi-vehicle 3D space-time intervals, two key performance indicators are constructed: the breadth of the driving interval and the deviation from the reference trajectory

[0122]

[0123] Where k represents the time period number. represents the range breadth weight of unmanned vehicle i; time period The drivable area is given by To characterize, see Figure 3 (c) is the maximum and minimum value of the drivable range in the x and y directions; by maximizing Ensure that the planned drivable area is the largest. Joint optimization is needed.

[0124] The 3D space-time constraint layer integrates dynamic and static obstacle information to construct a 3D space-time map, segment the time axis, and define the drivable range for each vehicle within a time period. This area is no longer a simple fixed structure, but a more flexible boundary is generated through a dynamic constraint model based on the kinematic characteristics and real-time status of the vehicle.

[0125] Figure 3 (b) shows the change of the time period occupied by the three-dimensional space-time step. The deviation degree of the reference trajectory Specifically expressed as:

[0126]

[0127] in, Refers to the deviation weight of unmanned vehicle i, coordinate (x i (k),y i (k)) refers to the coordinate point of a specified key point within the drivable range, and the key point is preferably the centroid of the drivable range; is the reference trajectory in the 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 range is close to the reference trajectory.

[0128] Obviously, the driving range is wide is a linear function, the deviation of the reference trajectory is a quadratic convex function. The optimization of the three-dimensional space-time interval generates the cost of integrating all unmanned vehicles, and the final objective function is expressed as:

[0129]

[0130] Among them, η i is the weight of the unmanned vehicle i, indicating the priority. For the optimization problem of minimizing the objective function, since the goal of generating the three-dimensional space-time interval is to make the driving interval as large as possible, Preceded by a minus sign.

[0131] J corridor Minimum is the goal, jointly optimizing all unmanned vehicles and all time periods The drivable range is obtained to obtain the 3D space-time safety range of all unmanned vehicles. To avoid collisions when multiple unmanned vehicles interact, the areas represented in the 3D space-time map cannot overlap. During optimization, it is also necessary to restrict multiple vehicles from occupying the same spatial position in the same time period.

[0132] 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.

[0133] In a preferred solution, in order to improve the computing efficiency in high-density scenarios, a cache mechanism is introduced between the trajectory optimization layer and the three-dimensional space-time constraint layer, including success cache and failure cache.

[0134] Among them, the Success Cache is used to store verified safe paths and the corresponding three-dimensional space-time safety intervals; while the Failure Cache records infeasible paths and their corresponding three-dimensional space-time safety intervals, which are used to infer infeasibility.

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

[0136] If the current path is "pseudo-identical" to the infeasible path recorded in the failure cache, and its 3D space-time safety interval is a subset of or exactly the same as the 3D space-time safety interval recorded in the failure cache, then the current path can be directly judged as infeasible, and the trajectory optimization layer does not continue to plan the current path to avoid redundant calculations.

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

[0138] The dynamic adjustment of the regional boundaries of the three-dimensional spatiotemporal constraint layer combined with the caching mechanism effectively improves the computational efficiency and adaptability of path planning, and provides stable and efficient spatiotemporal constraints for the trajectory optimization layer.

[0139] (3) Trajectory Optimization Layer

[0140] The trajectory optimization layer performs local path planning based on the reference path, optimizes the control quantity according to the initial state information of the vehicle, optimizes the trajectory smoothness and comfort by considering the longitudinal and lateral coupling vehicle model constraints, and finally generates a safe trajectory suitable for vehicle tracking control.

[0141] Step S31: First, construct the global path feasibility constraint and the local path deviation rate constraint; by maximizing the global path feasibility, ensure that the planned path meets the vehicle capability and ride comfort; by minimizing the local path deviation rate, 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 constraints Related to path curvature:

[0144]

[0145] Among them, t represents the sampling time point of the trajectory optimization layer, and the time interval Δt is less than the unit time interval t(k)-t(k-1) of the space-time step in the three-dimensional space-time constraint layer. represents the trajectory smoothness weight of unmanned vehicle i. This scheme uses the path curvature The integral of the square term represents the smoothing performance. The smaller the curvature, the better the smoothing performance. Converted to front wheel angle To express. is the front wheel turning angle, and L is the vehicle wheelbase.

[0146] Trajectory comfort constraints Involving two aspects: path and speed:

[0147]

[0148] in, represents the rate of change of the front wheel turning angle of the unmanned vehicle i Weight, is the rate of change of acceleration Weight. Reflects ride comfort and is related to the rate of change of the front wheel angle and the rate of change of acceleration The amplitude values ​​are negatively correlated.

[0149] ② The local path deviation rate constraint is an interval boundary constraint, which uses the deviation degree of the key point Characterization, which is calculated in a three-dimensional space-time configuration space.

[0150]

[0151] in, and Respectively represent the deviation weights of the unmanned vehicle i from the key points in the x-axis and y-axis directions; It indicates that the node coordinates obtained by the trajectory optimization layer through local path planning of the time period in the reference trajectory sent by the three-dimensional space-time constraint layer are the objects to be optimized; (x i (k),y i (k)) indicates The key point coordinates corresponding to the drivable range; since the sampling time interval of the trajectory optimization layer is smaller than the unit time period length of the three-dimensional space-time constraint layer, the trajectory point coordinates within each time period k The key point coordinates of the space-time step (x i (k),y i (k)) performs time matching. By minimizing the local path deviation rate Ensure that the planned path is close to the reference trajectory issued by the 3D space-time constraint layer. Here, the key point refers to the key point specified in the drivable range, such as the centroid.

[0152] Step S32: adaptive dynamic weight calculation.

[0153] The trajectory optimization layer evaluates the global path feasibility and local path deviation rate, and dynamically adjusts the weights of centralized and distributed based on the current environment complexity and current communication bandwidth to adapt to different scenario requirements. The weight allocation formula is:

[0154] Global path feasibility constraint weight

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

[0156] Among them, C env is the complexity of the current environment, C max is the maximum threshold of environmental complexity; B comm Indicates the current communication bandwidth, B max is the maximum communication bandwidth.

[0157] In order to ensure the smoothness of weight adjustment and the stability of system operation, the present invention introduces a smooth switching mechanism to gradually transition between centralized and distributed weights to avoid the impact of drastic changes in weights on trajectory stability and vehicle motion control. The smooth switching mechanism introduces the time dimension to decompose the weight adjustment process into small incremental changes, gradually approaching the target weight value. Exponential smoothing has a faster adjustment speed in the early stage and gradually converges in the later stage, which is suitable for scenarios that are more sensitive to weight changes. W is updated at each sampling time t. distributed , the update formula is:

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

[0159] Among them, 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 -Wcentralized (t-1) is the weight value at time t-1 and the weight target value W calculated based on the environment complexity and communication bandwidth target The difference between them; λ is the convergence rate coefficient.

[0160] The trajectory optimization generation algorithm of each unmanned vehicle integrates the costs of equations (10), (11) and (12) as the final objective function:

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

[0162]

[0163] The trajectory optimization problem of the trajectory optimization layer under the three-dimensional space-time map takes formula (15) as the total objective function, takes the minimum total objective function as the goal, takes the longitudinal and lateral coupled vehicle kinematic model and the three-dimensional space-time corridor boundary as hard constraints, constructs the unmanned vehicle trajectory optimization model considering the vehicle model, and processes the nonlinear optimization problem to achieve joint optimization of each time period in the reference trajectory issued by the three-dimensional space-time constraint layer to obtain the optimal motion trajectory.

[0164] In summary, the present invention proposes an adaptive hierarchical multi-vehicle collaborative motion planning method based on three-dimensional space-time constraints. Through multi-level collaborative work, the centralized and distributed weights and smooth switching mechanisms are adaptively adjusted to ensure safety, real-time and collaborative efficiency in complex dynamic environments. The present invention generates behavioral primitives in the continuous time domain, which effectively solves the limitations of traditional discrete methods in smoothness and computational efficiency. In the space-time constraint layer, the space-time safety area is dynamically adjusted, and in the trajectory optimization layer, by considering factors such as vehicle dynamics and path smoothness, it is ensured that the generated trajectory meets both safety requirements and the vehicle's motion characteristics.

[0165] The above specific embodiments only describe the design principle of the present invention. The shapes and names of the components in the description may be different and are not limited. Therefore, those skilled in the art in the field of the present invention may modify or replace the technical solutions recorded in the above embodiments; and these modifications and replacements do not deviate from the creative purpose and technical solutions of the present invention and should all fall within the protection scope of the present invention.

Claims

1. An adaptive hierarchical multi-unmanned vehicle collaborative motion planning method under three-dimensional space-time constraints, characterized in that: include: Construct 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 set in the central controller to achieve global planning and collaborative coordination; the trajectory optimization layer is set in each unmanned vehicle to generate the optimal motion trajectory that meets the dynamic constraints; The behavior planning layer generates dynamic behavior primitives based on vehicle dynamics characteristics; Within the feasible domain of global map information, the optimal dynamic behavior primitive sequence is selected to generate the globally optimal reference trajectory of each unmanned vehicle; The three-dimensional space-time constraint layer generates the three-dimensional space-time safety interval of each unmanned vehicle based on the reference trajectory generated by the behavior planning layer, combined with the environment and vehicle status. The three-dimensional space-time safety interval is composed of the drivable interval corresponding to each time period on the trajectory; When generating a 3D space-time safety zone, the goal is to have a wide driving zone and a small deviation of the reference trajectory of key points in the driving zone. At the same time, the drivable zone should not interfere with the 3D space-time environment and multiple vehicles should not occupy the same spatial position in the same time period. The trajectory optimization layer implements local planning of the reference trajectory based on local perception under the constraints of the vehicle model and three-dimensional space-time safety interval, and generates the optimal motion trajectory for control tracking in real time.

2. The method according to claim 1, characterized in that In the behavior planning layer, based on the vehicle dynamics characteristics, the dynamic behavior primitives are generated as follows: The second-order state differential equation of the unmanned vehicle is combined with the nonlinear term to form the expression of the dynamic behavior primitive: Among them, y is the state of the unmanned vehicle, g is the target state of the task, τ controls the trajectory speed, and α y and β y is the gain coefficient, f(x) is the nonlinear term used to adjust the trajectory shape, and is represented by the weighted sum of basis functions.

3. The method according to claim 1, characterized in that In the behavior planning layer, within the feasible domain of the global map information, the optimal dynamic behavior primitive sequence is selected to generate the globally optimal reference trajectory of each unmanned vehicle: Design behavior primitive cost function including node cost C n (n) and edge cost C e (e pq ); Node cost C n (n) is measured by the difference between node n and the target point in the trajectory; Edge cost C e (e pq ) is the trajectory e between node p and node q in the trajectory pq It is measured by trajectory smoothness, task execution, human-like driving characteristics and overall traffic efficiency; Using mixed integer linear programming MILP, based on the node cost C n (n) and edge cost C e (e pq ) Jointly optimize the dynamic behavior primitive sequences of each unmanned vehicle to form the globally optimal reference trajectory of each unmanned vehicle.

4. The method according to claim 3, characterized in that The node cost Cn(n) is: C n (n)=ω distance ·dist(s n ,s target )+ω heading ·Δθ(s n ,s target ) Where 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 values; ω distance and ω heading Indicates the distance deviation weight and heading deviation weight; The edge cost C e (e pq )for: C e (e pq )=ω smooth ·J P +ω task ·C n (n q )+ω human ·C h (e pq )+ω efficient ·C v ||v e || Among them, e pq Represents node n in the trajectory p and node n q The edge between P is the path smoothness; C n (n q ) represents node n q The node cost of C h (e pq ) represents human-like driving characteristics, according to node n q To node n p The angle deviation is obtained by querying the human-like driving feature lookup table; C v ||v e || represents the overall traffic efficiency cost, and the scalar ||v e || is negatively correlated; ||v e || represents edge e pq The corresponding average vehicle speed; ω smooth ,ω task ,ω human and ω efficient represents the weight of the corresponding item; Edge cost C e (e pq ) in the path smoothness J P It is expressed as: Among them, t s and t g e pq The starting point and end point of the corresponding path; v(t) is the rear axle speed of the unmanned vehicle, δ(t) is the front wheel turning angle of the vehicle, and L is the wheelbase of the vehicle; c s is the steering control quantity, ω z is the yaw rate, which ensures the minimum overall curvature of the trajectory by minimizing the integral of the steering control amount and the yaw rate; Within the feasible domain of global map information, with the goal of minimizing the total objective function composed of node cost, edge cost, and path smoothness, the MILP model is used to jointly optimize the dynamic behavior primitive sequences of all unmanned vehicles to generate the globally optimal reference trajectories of each unmanned vehicle.

5. The method according to claim 1, characterized in that In the three-dimensional space-time constraint layer, the method of generating the three-dimensional space-time safety interval is as follows: 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 recorded as the time period Build key performance indicators, including driving range breadth and the deviation from the reference trajectory The breadth of the driving range for: Where k represents the time period number; represents the range breadth weight of unmanned vehicle i; time period The drivable area is given by to characterize; is the maximum and minimum value of the x and y directions of the drivable range; by maximizing Ensure that the planned drivable area is the largest overall; The deviation from the reference trajectory for: in, Refers to the deviation weight of unmanned vehicle i, coordinate (x i (k), yi(k)) refers to the time period The coordinates of the specified key points in the corresponding drivable range, For time period The coordinates of the sampling time points of the internal reference trajectory; by minimizing Ensure that the planned drivable range is close to the reference trajectory; The optimization of the three-dimensional space-time safety interval generates the cost of integrating all unmanned vehicles, and the final objective function is expressed as: Among them, η i is the weight of unmanned vehicle i, indicating the priority; J corridor The goal is to minimize the number of unmanned vehicles and to limit the interference with the three-dimensional space-time environment. Multiple vehicles are not allowed to occupy the same space in the same time period. All unmanned vehicles and all time periods are jointly optimized. The drivable range is obtained to obtain the three-dimensional space-time safety range of all unmanned vehicles; 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.

6. The method according to claim 1, characterized in that In the trajectory optimization layer, local planning is performed on the reference trajectory as follows: Construct global path feasibility constraints and local path deviation rate constraints; maximize global path feasibility to ensure that the planned path meets vehicle capabilities 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; Dynamically adjust the weight of the global path feasibility constraint and the weight of the local path deviation rate constraint according to the current environment complexity and the current communication bandwidth; Using weights, the global path feasibility constraint and the local path deviation rate constraint are weighted to obtain the overall objective function of the optimal motion trajectory of the unmanned vehicle; With the goal of minimizing the total objective function of the optimal motion trajectory, 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.

7. The method according to claim 6, characterized in that The global path feasibility constraints include trajectory smoothness constraints and trajectory comfort constraints The trajectory smoothness constraint for: Among them, t represents the sampling time point of the trajectory optimization layer, represents the trajectory smoothness weight of unmanned vehicle i; is the front wheel turning angle, L is the vehicle wheelbase; The trajectory comfort constraint for: in, represents the rate of change of the front wheel turning angle of the unmanned vehicle i The weight of is the acceleration change rate of unmanned vehicle i The weight of The local path deviation rate constraint for: in, Represents the time period in the reference trajectory of the unmanned vehicle i issued by the trajectory optimization layer to the three-dimensional space-time constraint layer The node coordinates obtained by local path planning are the objects that need to be optimized; express Coordinates of key points in the drivable area; and Respectively represent the deviation weights of the unmanned vehicle i from the key points in the x-axis and y-axis directions; by minimizing the local path deviation rate Make the planned local path close to the reference trajectory within the drivable range.

8. The method according to claim 6, characterized in that The weight of the global path feasibility constraint W centralized The weight W of the local path deviation rate constraint distributed They are: IN distributed =1-W centralized Among them, C env is the complexity of the current environment, C max is the maximum threshold of environmental complexity; B comm Indicates the current communication bandwidth, B max is the maximum communication bandwidth.

9. The method according to claim 8, characterized in that W distributed Changes over time, and updates W at each sampling time t distributed , the update formula is: W centralized (t)=W centralized (t-1)+ΔW·e -λt Among them, 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 weight value at time t-1 and the weight target value W calculated based on the environment complexity and communication bandwidth target The difference between them; λ is the convergence rate coefficient.

10. The method according to claim 8, characterized in that A cache mechanism is further provided between the three-dimensional space-time constraint layer and the trajectory optimization layer, including a success cache and a failure cache; The successful cache stores the verified safe paths and corresponding three-dimensional space-time safe intervals of the trajectory optimization layer; The infeasible paths fed back by the trajectory optimization layer and the corresponding three-dimensional space-time safety intervals are stored in the failure buffer; If the current path matches a safe path in the successful cache, and the three-dimensional safe space-time interval is exactly the same, the trajectory optimization layer directly reuses the trajectory stored in the successful cache without recalculation; If the current path matches the infeasible path recorded in the failure cache, and the 3D space-time safety interval of the current path is a subset of or exactly the same as the 3D space-time safety interval recorded in the failure cache, then the current path is directly judged to be 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