Path collaborative planning method for hybrid vehicle group conflict area

Abstract

The invention provides a path collaborative planning method for a hybrid vehicle group conflict area, and belongs to the technical field of vehicle path planning. According to the method, the virtual following target point of the driver is obtained by sampling the speed of the manual driving vehicle and the deviation path distance relative to the road reference line to generate the sampling track, and the driving behavior of the driver in the unstructured road environment is predicted; a forward deduction method of vehicle dynamics is combined to predict a short-time trajectory of the manually driven vehicle as a reference trajectory to evaluate the confidence coefficient of the reference trajectory, so that the accuracy of the sampling trajectory can be improved, and the collaboration and safety among the vehicles are improved; besides, in combination with information of the space-time risk field, the priority and the driving track of the unmanned vehicle are dynamically adjusted by adopting a space-time hybrid A star algorithm based on a dynamic risk field, so that efficient dynamic redistribution of space-time resources in a conflict area scene is realized, and the collision risk of the vehicle in the conflict area is reduced. According to the method, the multi-vehicle cooperative operation stability in the mixed scene can be remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle path planning, and particularly relates to a path collaborative planning method for a conflict area of a mixed vehicle group. Background Art

[0002] The multi-vehicle collaborative planning technology refers to coordinating the movement trajectories of multiple vehicles in scenarios such as port and mine operations to ensure that they can safely and efficiently complete tasks in conflict areas such as intersections.

[0003] In actual application scenarios, due to considerations of progressive automation, some driverless vehicles are often introduced first to cooperate with traditional human-driven vehicles, forming a mixed vehicle group of human-driven vehicles and driverless vehicles. In the multi-vehicle collaborative operation of the mixed vehicle group, not only the efficiency of collaborative planning needs to be concerned, but also the running status of the vehicle group needs to be monitored in real time. However, the driving behavior of the driver in the human-driven vehicle has certain randomness, which may destroy the result of collaborative planning and increase the risk of vehicle collision. And in unstructured scenarios such as ports and mines, due to the lack of sufficient driving behavior data, it is difficult to accurately model the driving behavior of the driver, making it difficult to accurately predict the trajectory of the human-driven vehicle and unable to effectively plan a reasonable path for the mixed vehicles.

[0004] Therefore, it is necessary to provide a path collaborative planning method for a conflict area of a mixed vehicle group to solve the above problems. Summary of the Invention

[0005] The present invention provides a path collaborative planning method for a conflict area of a mixed vehicle group, which monitors the running state of the mixed vehicle group in real time based on a cloud platform, predicts the random driving behavior of the human-driven vehicle, and quantitatively evaluates the potential risks caused by it. On this basis, the priority and driving trajectory of the driverless vehicle are dynamically adjusted by using a spatio-temporal hybrid A* algorithm based on a dynamic risk field, realizing the efficient dynamic reallocation of spatio-temporal resources in the conflict area scene and reducing the collision risk of vehicles in the conflict area, which can effectively solve at least one technical problem involved in the background art.

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

[0007] A path collaborative planning method for a conflict area of a mixed vehicle group, the mixed vehicle group including human-driven vehicles and driverless vehicles, the path collaborative planning method for the conflict area of the mixed vehicle group comprising the following steps:

[0008] Step S1, sampling the speed of the human-driven vehicle and the deviation path distance relative to the road reference line to obtain the virtual following target point of the driver, modeling the driving behavior of the driver, and generating the sampling trajectory of the human-driven vehicle under the random driving behavior;

[0009] Step S2, predict the short-term trajectory of the human-driven vehicle as the reference trajectory based on the forward deduction method of vehicle dynamics;

[0010] Step S3, calculate the smoothness cost of the sampled trajectory and the similarity cost between the sampled trajectory and the reference trajectory, form the total cost by combining the smoothness cost and the similarity cost, evaluate the confidence of each sampled trajectory, and determine the trajectory prediction result of the human-driven vehicle based on the evaluation result of the confidence;

[0011] Step S4, discretize the spatio-temporal resources in the conflict area, calculate the collision probability between the driverless vehicle and the human-driven vehicle under the predetermined trajectory according to the trajectory prediction result of the human-driven vehicle, provide real-time risk warning for the human-driven vehicle based on the calculated collision risk estimate, and put forward adjustment suggestions; at the same time, readjust the priority of the affected driverless vehicles to eliminate the trajectory conflict between the driverless vehicle and the human-driven vehicle;

[0012] Step S5, introduce the risk cost of the extended step size in the spatio-temporal hybrid A* algorithm, evaluate the cumulative collision risk of the future trajectory of the driverless vehicle from the current node to the end point in the spatio-temporal resources, and generate a collision-free smooth trajectory for the driverless vehicle.

[0013] As a preferred improvement, step S1 specifically includes the following steps:

[0014] Generate the sampling speed of the human-driven vehicle by setting a floating value using the reference speed obtained by the roadside unit; select the discrete deviation set near 0 as the sampling lateral deviation of the human-driven vehicle; generate the trajectory of the human-driven vehicle in the Frenet coordinate system;

[0015] Based on the road reference line, convert the trajectory of the human-driven vehicle in the Frenet coordinate system to the Cartesian coordinate system, and the conversion relationship is expressed as:

[0016]

[0017] In the formula, x ref 、y ref respectively represent the horizontal and vertical coordinates of the projection point of the sampled trajectory point of the human-driven vehicle i on the road reference line in the Cartesian coordinate system; ψ s represents the heading angle of the human-driven vehicle i at the sampling time t s ; respectively represent the horizontal and vertical coordinates of the sampled trajectory point of the human-driven vehicle i in the Cartesian coordinate system at the sampling time t s ;

[0018] The velocity components of the human-driven vehicle i in the Cartesian coordinate system are obtained by synthesizing the velocity and angular velocity in the Frenet coordinate system:

[0019]

[0020] Wherein, respectively represent the velocity components of the human-driven vehicle i in the x-axis and y-axis directions of the Cartesian coordinate system at the sampling time t s ; s s '(t s ), d s '(t s ) respectively represent the first-order differentials of the abscissa and ordinate of the sampling trajectory point of the human-driven vehicle i in the Frenet coordinate system at the sampling time t s ; θ s represents the yaw angle of the human-driven vehicle i in the Cartesian coordinate system at the sampling time t s , which is expressed as:

[0021]

[0022] Wherein, κ represents the curvature of the road reference line;

[0023] At the sampling time t s , the velocity of the human-driven vehicle i in the Cartesian coordinate system is expressed as:

[0024]

[0025] After obtaining the termination sampling states in the x-axis and y-axis directions of the Cartesian coordinate system, the trajectories of the human-driven vehicle i in the x-axis and y-axis directions of the Cartesian coordinate system are respectively generated using fifth-order polynomials, and the generation processes are respectively expressed as:

[0026]

[0027]

[0028] Wherein, respectively represent the trajectories of the human-driven vehicle i in the x-axis and y-axis directions of the Cartesian coordinate system at the sampling time t s ; a0, a1, …, a5 and b0, b1, …, b5 all represent polynomial coefficients.

[0029] As a preferred improvement, the sampling trajectory generated by the human-driven vehicle i using the fifth-order polynomial is expressed as:

[0030]

[0031] Wherein, respectively represent the lateral coordinate, longitudinal coordinate, speed, acceleration, and yaw rate of the human-driven vehicle i in the Cartesian coordinate system at the sampling time t s s

[0032] As a preferred improvement, step S2 specifically includes the following steps:

[0033] Obtain the short-term trajectory of the human-driven vehicle using a constant turn rate and acceleration model. The state space and state transition of the system are expressed as:

[0034]

[0035] In the formula, represents the state space of the human-driven vehicle i at time t c c respectively represent the lateral coordinate, longitudinal coordinate, speed, acceleration, and yaw rate of the human-driven vehicle i in the Cartesian coordinate system at time t c c represents the state space of the human-driven vehicle i after a running period of Δt; T represents the transpose matrix; c c represents the state transition quantity, obtained through CTRA kinematics, and is expressed as:

[0036]

[0037] In the formula, represents the yaw angle of the human-driven vehicle i at time t c c represents the yaw rate of the human-driven vehicle i at time t c c

[0038] The reference trajectory of the human-driven vehicle i predicted based on vehicle dynamics is expressed as:

[0039]

[0040] As a preferred improvement, the trajectory smoothness cost includes a lateral smoothness cost and a longitudinal smoothness cost. Among them, the lateral smoothness cost J lat achieves the smoothness of the trajectory by minimizing the lateral acceleration and lateral jerk. The longitudinal smoothness cost J lon represents the constraint on the longitudinal speed change rate;

[0041] The lateral smoothness cost J of the human-driven vehicle i lat is expressed as:

[0042]

[0043] In the formula, represents the lateral acceleration of the manually-driven vehicle i in the Cartesian coordinate system, represents the lateral jerk of the manually-driven vehicle i in the Cartesian coordinate system, which is the rate of change of the lateral acceleration over time and is obtained by taking the derivative of and is expressed as:

[0044]

[0045] The longitudinal smoothness cost J lon is expressed as:

[0046]

[0047] In discrete time, it is calculated by the velocity difference of adjacent sampled trajectory points and is expressed as:

[0048]

[0049] where, respectively represent the velocities of the manually-driven vehicle i at the sampled trajectory points j and j + 1 in the Cartesian coordinate system; Δt s represents the time interval between the sampled trajectory points j and j + 1;

[0050] Then the trajectory smoothness cost of the manually-driven vehicle i is expressed as:

[0051]

[0052] where, w lat and w lon respectively represent the weight coefficients of J lat and J lon respectively.

[0053] As a preferred improvement, the similarity cost between the reference trajectory and the sampled trajectory includes the position similarity cost, the velocity similarity cost, the acceleration similarity, and the angular velocity similarity cost; among them, the position similarity cost uses the Euclidean distance to evaluate the spatial deviation of the trajectory points on the reference trajectory and the sampled trajectory, the velocity similarity cost compares the mean square error of the velocity characteristics of the sampled trajectory and the reference trajectory, the acceleration similarity cost evaluates the mean square error of the acceleration characteristics, and the angular velocity similarity cost evaluates the mean square error of the angular velocity characteristics;

[0054] The position similarity cost between the k-th sampled trajectory of the manually-driven vehicle i and the reference trajectory is expressed as:

[0055]

[0056] wherein, m represents a trajectory point on the k-th sampling trajectory of the manually-driven vehicle i; N represents the number of trajectory points on the k-th sampling trajectory of the manually-driven vehicle i;

[0057] The speed similarity cost between the k-th sampling trajectory of the manually-driven vehicle i and the reference trajectory is expressed as:

[0058]

[0059] The acceleration similarity cost between the k-th sampling trajectory of the manually-driven vehicle i and the reference trajectory is expressed as:

[0060]

[0061] The angular velocity similarity cost between the k-th sampling trajectory of the manually-driven vehicle i and the reference trajectory is expressed as:

[0062]

[0063] Then the similarity cost between the k-th sampling trajectory of the manually-driven vehicle i and the reference trajectory is expressed as:

[0064]

[0065] As a preferred improvement, the total cost of the k-th sampling trajectory of the manually-driven vehicle i is expressed as:

[0066]

[0067] The confidence of the k-th sampling trajectory of the manually-driven vehicle i is expressed as:

[0068]

[0069] wherein, represents the probability of the k-th sampling trajectory of the manually-driven vehicle i.

[0070] As a preferred improvement, whether there is a collision risk between the driverless vehicle and the manually-driven vehicle is judged by detecting the Euclidean distance between the predetermined trajectory of the driverless vehicle and the predicted trajectory of the manually-driven vehicle. The judgment of the collision condition is as follows: within the prediction time window t ∈ T window , if the Euclidean distance between the predetermined trajectory point of the driverless vehicle and the predicted trajectory point of the manually-driven vehicle is less than the set safety distance threshold d safe , it is considered that there is a trajectory coincidence, and thus there is a collision risk:

[0071]

[0072] In the formula, Colision(T i ,T ego ,t) represents the collision risk between the driverless vehicle and the human-driven vehicle at time t. The value is 1, indicating that there is a collision risk between the driverless vehicle and the human-driven vehicle; the value is 0, indicating that there is no collision risk between the driverless vehicle and the human-driven vehicle; T ego represents the predetermined trajectory of the driverless vehicle; x ego (t), y ego (t) respectively represent the horizontal and vertical coordinates of the predetermined trajectory point of the driverless vehicle at time t;

[0073] The overall collision risk estimation of the predetermined trajectory of the driverless vehicle and the predicted trajectory of the human-driven vehicle needs to traverse all trajectory points on the trajectory, and is weighted and summed by the risk values within the time window T window =[t0,t f . The collision probability is expressed as:

[0074]

[0075] In the formula, R colision (T i ) represents the collision probability between the predetermined trajectory of the driverless vehicle and the predicted trajectory of the human-driven vehicle; ζ(t) represents the time weight, which is designed as an exponential decay function and is expressed as:

[0076]

[0077] In the formula, λ represents the decay coefficient;

[0078] The total collision probability of the predetermined trajectory of the driverless vehicle and the set of predicted trajectories of the human-driven vehicle is obtained by comprehensively considering the confidence and collision probability of each predicted trajectory, and is expressed as:

[0079]

[0080] In the formula, K represents the total number of predicted trajectories of the human-driven vehicle i;

[0081] When the total collision risk probability of the full trajectory set predicted by the driverless vehicle and the human-driven vehicle exceeds the set threshold, that is, R colision,total >R safe , the occupancy prediction trajectory set within the future time t∈T window is mapped into the spatio-temporal resource field to form a spatio-temporal risk cost field.

[0082] As a preferred improvement, the construction of the spatio-temporal risk cost field is as follows:

[0083] Quantify the risk value of each space-time point in the space-time resource field. For each moment t and each spatial position (x, y), define its basic risk value It is expressed as:

[0084]

[0085] Traverse the involved space-time points to obtain the space-time risk cost field. By comprehensively considering the occupancy probability of each trajectory point and its distribution characteristics in the space-time resource field, this space-time risk cost field reflects the collision risk level of different spatial positions at the current moment.

[0086] As a preferred improvement, the space-time hybrid A-star algorithm represents the expanded state node as: s(x, y, θ, t), where: x and y respectively represent the lateral coordinate and longitudinal coordinate of the driverless vehicle; θ is the heading angle of the driverless vehicle; t is the time;

[0087] For each exploration, generate a position update that satisfies its kinematic constraints as follows:

[0088] Δd = v n+1 ·ΔT;

[0089]

[0090] θ n+1 = θ n +Δθ;

[0091] x n+1 = x n + R r ·(sinθ n+1 - sinθ n );

[0092] y n+1 = y n + R r ·(cosθ n - cosθ n+1 );

[0093] t n+1 = t n +ΔT;

[0094] In the formula, ΔT represents the time resolution; Δd represents the change in the driving distance of the driverless vehicle; v n+1 represents the speed of the driverless vehicle at the n + 1 node; Δθ represents the change in the steering angle of the driverless vehicle; δ j represents the front wheel steering angle of the driverless vehicle; L represents the wheelbase of the driverless vehicle; θ n+1 、θ n respectively represent the heading angles of the driverless vehicle at the n + 1 node and the n node; xn+1 , y n+1 respectively represent the lateral coordinates of the driverless vehicle at the n+1 node and the n node; y n+1 , y n respectively represent the longitudinal coordinates of the driverless vehicle at the n+1 node and the n node; R r represents the turning radius of the driverless vehicle; where:

[0095]

[0096] To ensure the rationality of the driverless vehicle control, constraints on acceleration, steering angle, and speed are set, expressed as:

[0097] a min ≤ a i ≤ a max ;

[0098] -δ max ≤ δ j ≤ δ max ;

[0099] 0 ≤ v n+1 ≤ v max .

[0100] In the spatio-temporal hybrid A* algorithm, the risk cost c(n) of the extended step size is introduced to evaluate the cumulative collision risk of the future trajectory from the current node to the end point in spatio-temporal resources. This future extended trajectory is assumed to be a uniform Cartesian path from the current point to the end point. The spatio-temporal heuristic cost of the current node is:

[0101] c(n) = ∑ζ(t)Colision(T i , T ego , t);

[0102] Therefore, the total cost function f(n) of the spatio-temporal hybrid A* based on the dynamic risk field is expressed as:

[0103] f(n) = h(n) + g(n) + c(n);

[0104] In the formula, h(n) represents the expected future cost from the current node to the target node; g(n) represents the cumulative historical cost from the starting node to the current node; c(n) represents the spatio-temporal heuristic cost of the current node, where:

[0105] h(n) = ||s n - s G ||2 = |x n - x G | + |y n - y G | + |t n - t G |;

[0106] In the formula, s n and s G respectively represent the spatio-temporal coordinates of the current node and the target node, s n =(x n , y n , t n ), s G =(x G , y G , t G );

[0107] g(n)=g(n - 1)+Δd+w v ·|v n -v n-1 |+w ref ·|v n -v ref |+w δ ·|θ n -θ n-1 |;

[0108] After predicting the rough trajectory of the driverless vehicle based on the dynamic risk field using the spatio-temporal hybrid A-star algorithm, the path and speed are smoothed to obtain a more comfortable trajectory, where:

[0109] The path optimization objective function F p is expressed as:

[0110] minF p =ω s ·f s (X)+ω r ·f r (X)+ω l ·f l (X);

[0111] In the formula, f s (X) represents the path smoothing cost, which is used to reduce sharp turns or irregular changes in the trajectory to improve the smoothness of driving; f r (X) represents the path fitting original trajectory cost, which is used to make the generated smooth trajectory as close as possible to the original trajectory, thus maintaining the rationality and accuracy of the trajectory; f l (X) represents the path uniformity cost, which is used to optimize the overall balance of the trajectory, making the acceleration change smoothly during driving and avoiding frequent acceleration or deceleration; ω s , ω r , ω l respectively represent the weight coefficients of f s (X), f r (X), f l (X);

[0112] Velocity optimization objective function F v It is expressed as:

[0113] minF v = ω v ·f v (S)+ ω a ·f a (S)+ ω jerk ·f jerk (S);

[0114] In the formula, f v (S) represents the speed deviation cost, f v (s)= ∑(v k - v ref ) 2 ; f a (S) represents the acceleration change cost, f jerk (S) represents the jerk change cost, f jerk (S)= ∑(a k+1 - a k ) 2 .

[0115] The beneficial effects of the present invention are as follows:

[0116] (1) The present invention samples the speed of the manually driven vehicle and the deviation path distance relative to the road reference line to obtain the virtual following target point of the driver, generates a sampling trajectory, predicts the driving behavior of the driver in an unstructured road environment, and combines the forward deduction method of vehicle dynamics to predict the short-term trajectory of the manually driven vehicle as a reference trajectory to evaluate the confidence of the reference trajectory, which can improve the accuracy of the sampling trajectory, thereby improving the coordination and safety between vehicles;

[0117] (2) Based on the spatio-temporal hybrid A* algorithm, a safe, collision-free, and smooth trajectory that can be adapted in real time in a complex dynamic environment is generated for the driverless vehicle in the spatio-temporal graph. Combining the information of the spatio-temporal risk field, through the real-time evaluation and risk analysis of the conflict area, the decision-making process of the trajectory planning of the driverless vehicle is optimized, effectively avoiding the collision risk and improving the driving safety and efficiency of the driverless vehicle in complex scenarios. Description of the Drawings

[0118] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained according to these drawings, where:

[0119] Figure 1 Schematic diagram showing the conflict area of unstructured roads

[0120] Figure 2 Flow framework diagram showing a path collaborative planning method for the conflict area of a hybrid vehicle group provided by the present invention Detailed implementation manners

[0121] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention

[0122] As Figure 1 shown, in the working scenario involved in the present invention, there is a hybrid vehicle group of manually driven vehicles and driverless vehicles, and all vehicles perform real-time data interaction and information communication with the cloud platform through in-vehicle communication modules. The main functions of the in-vehicle communication module include receiving instructions distributed by the cloud platform and uploading the dynamic states (such as position, speed, acceleration, etc.) of the vehicles to the cloud platform to support the global optimization and efficient management of the overall traffic flow by the cloud platform

[0123] For manually driven vehicles, the roadside unit sends instructions to pass through the intersection to the manually driven vehicle, and the instruction distributed to the manually driven vehicle i is expressed as

[0124]

[0125] In the formula, P i represents whether to allow the manually driven vehicle i to pass through the intersection, P i ∈{0, 1}, P i = 1 means allowing passage, and P i = 0 means waiting to pass represents the recommended passing speed, which is calculated by the roadside unit according to the current road conditions and the dynamic states of the manually driven vehicles; R i represents the recommended passing route, guiding the vehicle to pass through the intersection along the specified path to avoid route conflicts with other vehicles

[0126] After receiving the instructions, the driver of the manually driven vehicle performs a trajectory following operation based on the road reference line of the lane scene. The cloud platform, according to the predetermined task instructions, allocates corresponding spatio-temporal resources to the manually driven vehicle to ensure that it can safely pass through the intersection according to the planned path and time window

[0127] After allocating time and space resources to all manually driven vehicles, the unmanned vehicle uses a combination of centralized decision-making and distributed planning to determine the time and space resource usage priority of the unmanned vehicle on the cloud platform. Based on this priority, the unmanned vehicle uses an optimization search algorithm to generate its own optimal driving trajectory, thereby completing the intersection in a safe and efficient manner.

[0128] However, due to the certain randomness of the driving behavior of drivers in manually driven vehicles, there is a risk that manually driven vehicles will deviate from the predetermined trajectory or violate the instructions of the cloud platform, which in turn makes the original space-time resource allocation plan invalid, and has an adverse impact on the overall traffic efficiency and safety of intersections in mixed traffic environments.

[0129] See also Figure 2 In order to solve the above problems, this embodiment provides a path coordination planning method for a mixed vehicle group conflict area, comprising the following steps:

[0130] Step S1, sampling the speed of the manually driven vehicle and the deviation path distance relative to the road reference line to obtain the driver's virtual following target point, modeling the driver's driving behavior, and generating a sampling trajectory of the manually driven vehicle under random driving behavior.

[0131] The present application obtains the driver's virtual following target point by sampling the speed of the manually driven vehicle and the deviation path distance relative to the road reference line (the center line of the road), that is, the driver's driving behavior is modeled by following the front target point during the driving process, which conforms to the driving habits of human drivers and can achieve accurate modeling of the driver's driving behavior.

[0132] The sampled speed of manually driven vehicles is generated by setting a floating value using the reference speed obtained from the roadside unit; the sampled lateral deviation is set to a discrete deviation set near 0 to capture the randomness of the driver's driving in unstructured scenarios.

[0133] Based on the sampled speed and sampled lateral deviation, the trajectory of the artificially driven vehicle in the Frenet coordinate system is generated. end ), any sampling time t s When the path point coordinates of the manually driven vehicle i are expressed as (s s ,d s ), where s s =v s t s , sampling speed v s and sampling lateral deviation d s Different values ​​can produce different sampling trajectories, modeling different trajectories that may be generated under random driving behavior.

[0134] At this time, the sampling trajectory is generated based on the Frenet coordinate system. For the convenience of subsequent calculations, it is also necessary to convert it to the Cartesian coordinate system. The specific conversion process is as follows:

[0135] Based on the road reference line, the trajectory of the human-driven vehicle in the Frenet coordinate system is converted to the Cartesian coordinate system. The conversion relationship is expressed as:

[0136]

[0137] In the formula, x ref , y ref respectively represent the abscissa and ordinate of the projection point of the sampling trajectory point of the human-driven vehicle i on the road reference line in the Cartesian coordinate system; ψ s represents the heading angle of the human-driven vehicle i at the sampling time t s ; respectively represent the abscissa and ordinate of the sampling trajectory point of the human-driven vehicle i in the Cartesian coordinate system at the sampling time t s ;

[0138] The velocity components of the human-driven vehicle i in the Cartesian coordinate system are obtained by synthesizing the velocity and angular velocity in the Frenet coordinate system:

[0139]

[0140] In the formula, respectively represent the velocity components of the human-driven vehicle i in the x-axis and y-axis directions of the Cartesian coordinate system at the sampling time t s ; s s '(t s ), d s '(t s ) respectively represent the first-order differentials of the abscissa and ordinate of the sampling trajectory point of the human-driven vehicle i in the Frenet coordinate system at the sampling time t s ; θ s represents the yaw angle of the human-driven vehicle i in the Cartesian coordinate system at the sampling time t s , which is expressed as:

[0141]

[0142] In the formula, κ represents the curvature of the road reference line;

[0143] At the sampling time t s , the velocity of the human-driven vehicle i in the Cartesian coordinate system is expressed as:

[0144]

[0145] After obtaining the termination sampling states in the x-axis and y-axis directions in the Cartesian coordinate system, the trajectories of the human-driven vehicle i in the x-axis and y-axis directions of the Cartesian coordinate system are respectively generated using fifth-degree polynomials, and the generation processes are respectively expressed as:

[0146]

[0147] In the formula, respectively represent the sampling time t s When, the trajectories of the human-driven vehicle i in the x-axis and y-axis directions of the Cartesian coordinate system; a0, a1, …, a5 and b0, b1, …, b5 all represent polynomial coefficients.

[0148] Substitute the positions, velocities, and accelerations of the human-driven vehicle at the initial state and the termination state into the above formula, and the polynomial coefficients a0, a1, …, a5 and b0, b1, …, b5 can be solved. The initial position at the initial state Initial velocity Initial acceleration are obtained by transmission from the roadside unit; the termination position at the termination state Velocity are obtained from the above sampling process, and the termination acceleration is 0.

[0149] The sampling trajectory of the human-driven vehicle i generated by the fifth-degree polynomial is expressed as:

[0150]

[0151] In the formula, respectively represent the sampling time t s When, the lateral coordinate, longitudinal coordinate, velocity, acceleration, and yaw rate of the human-driven vehicle i in the Cartesian coordinate system.

[0152] Step S2, predict the short-term trajectory of the human-driven vehicle as the reference trajectory based on the forward deduction method of vehicle dynamics.

[0153] This application uses CTRA (constant turn rate and acceleration model) to obtain the short-term trajectory of the human-driven vehicle i, and the state space and state transition of the system are expressed as:

[0154]

[0155] In the formula, represents the state space of the human-driven vehicle i at time t c ; respectively represent t cThe lateral coordinate, longitudinal coordinate, speed, acceleration, and yaw rate of the human-driven vehicle i in the Cartesian coordinate system at a moment; Denote Δt c The state space of the human-driven vehicle i after a running period; T represents the transpose matrix; Denote the state transition quantity, obtained through the CTRA kinematics, expressed as:

[0156]

[0157] In the formula, Denote t c The yaw angle of the human-driven vehicle i at the moment; Denote t c The yaw rate of the human-driven vehicle i at the moment.

[0158] The reference trajectory of the human-driven vehicle i predicted based on vehicle dynamics is expressed as:

[0159]

[0160] Step S3, calculate the smoothness cost of the sampled trajectory and the similarity cost between the sampled trajectory and the reference trajectory, form the total cost by combining the smoothness cost and the similarity cost, evaluate the confidence of each sampled trajectory, and determine the trajectory prediction result of the human-driven vehicle based on the evaluation result of the confidence.

[0161] There may be multiple solutions in the process of calculating the quintic polynomial, that is, there are multiple possible sampled trajectories. Due to the driving habits of human drivers, the driving trajectory is generally kept smooth. Therefore, this application calculates the trajectory smoothness cost to reflect the trend of the driver using this trajectory. The higher the trajectory smoothness cost, the greater the trend of the driver using this trajectory, and this is used as the basis for determining the sampled trajectory.

[0162] The trajectory smoothness cost includes the lateral smoothness cost and the longitudinal smoothness cost. Among them, the lateral smoothness cost realizes the smoothness of the trajectory by minimizing the lateral acceleration and the lateral jerk, and the longitudinal smoothness cost represents the constraint on the longitudinal speed change rate.

[0163] The lateral smoothness cost J of the human-driven vehicle i lat Is expressed as:

[0164]

[0165] In the formula, Denote the lateral acceleration of the human-driven vehicle i in the Cartesian coordinate system, Denote the lateral jerk of the human-driven vehicle i in the Cartesian coordinate system, which is the change rate of the lateral acceleration over time, by taking the Obtained by differentiation, expressed as:

[0166]

[0167] Longitudinal smoothness cost J lon Expressed as:

[0168]

[0169] In discrete time, Calculated by the velocity difference of adjacent sampled trajectory points, expressed as:

[0170]

[0171] In the formula, respectively represent the velocities of the human-driven vehicle i at the sampled trajectory points j and j + 1 in the Cartesian coordinate system; Δt s represents the time interval between the sampled trajectory points j and j + 1.

[0172] Then the trajectory smoothness cost of the human-driven vehicle i is expressed as:

[0173]

[0174] In the formula, w lat and w lon respectively represent the weight coefficients of J lat and J lon respectively.

[0175] The reference trajectory based on kinematic prediction is accurate in a short time domain. Therefore, the present invention compares whether the sampled trajectory generated by the quintic polynomial is reasonable through the reference trajectory, and evaluates the confidence of each sampled trajectory by calculating the similarity between the reference trajectory and the sampled trajectory.

[0176] The similarity cost between the reference trajectory and the sampled trajectory includes position similarity cost, velocity similarity cost, acceleration similarity, and angular velocity similarity cost. Among them, the position similarity cost uses the Euclidean distance to evaluate the spatial deviation of the trajectory points on the reference trajectory and the sampled trajectory, the velocity similarity cost compares the mean square error of the velocity characteristics of the sampled trajectory and the reference trajectory, the acceleration similarity cost evaluates the mean square error of the acceleration characteristics, and the angular velocity similarity cost evaluates the mean square error of the angular velocity characteristics.

[0177] The position similarity cost between the k-th sampled trajectory of the human-driven vehicle i and the reference trajectory is expressed as:

[0178]

[0179] Wherein, m represents the trajectory point on the k-th sampling trajectory of the manned vehicle i; N represents the number of trajectory points on the k-th sampling trajectory of the manned vehicle i;

[0180] The speed similarity cost between the k-th sampling trajectory of the manned vehicle i and the reference trajectory Is expressed as:

[0181]

[0182] The acceleration similarity cost between the k-th sampling trajectory of the manned vehicle i and the reference trajectory Is expressed as:

[0183]

[0184] The angular velocity similarity cost between the k-th sampling trajectory of the manned vehicle i and the reference trajectory Is expressed as:

[0185]

[0186] Then the similarity cost between the k-th sampling trajectory of the manned vehicle i and the reference trajectory Is expressed as:

[0187]

[0188] The total cost of the k-th sampling trajectory of the manned vehicle i Is expressed as:

[0189]

[0190] The confidence of the k-th sampling trajectory of the manned vehicle i is expressed as:

[0191]

[0192] Wherein, Represents the probability of the k-th sampling trajectory of the manned vehicle i.

[0193] Step S4: Discretize the spatio-temporal resources in the conflict area, calculate the collision probability between the unmanned vehicle and the manned vehicle under the predetermined trajectory according to the trajectory prediction result of the manned vehicle, provide real-time risk warning for the manned vehicle based on the calculated collision risk estimate, and propose adjustment suggestions; at the same time, readjust the priority of the affected unmanned vehicles to eliminate the trajectory conflict between the unmanned vehicle and the manned vehicle.

[0194] Whether there is a collision risk between the driverless vehicle and the human-driven vehicle is determined by detecting the Euclidean distance between the predetermined trajectory of the driverless vehicle and the predicted trajectory of the human-driven vehicle. The judgment of the collision condition is as follows: within the prediction time window \(t\in T\). window , if the Euclidean distance between the predetermined trajectory point of the driverless vehicle and the predicted trajectory point of the human-driven vehicle is less than the set safety distance threshold \(d\). safe , it is considered that there is a trajectory overlap, and thus there is a collision risk:

[0195]

[0196] In the formula, \(Colision(T\). i , \(T\). ego , \(t)\) represents the collision risk between the driverless vehicle and the human-driven vehicle at time \(t\). The value is 1, indicating that there is a collision risk between the driverless vehicle and the human-driven vehicle; the value is 0, indicating that there is no collision risk between the driverless vehicle and the human-driven vehicle; \(T\). ego represents the predetermined trajectory of the driverless vehicle; \(x\). ego (t), \(y\). ego (t) respectively represent the horizontal and vertical coordinates of the predetermined trajectory point of the driverless vehicle at time \(t\).

[0197] The overall collision risk estimation of the predetermined trajectory of the driverless vehicle and the predicted trajectory of the human-driven vehicle needs to traverse all trajectory points on the trajectory, and is obtained by weighted summation of the risk values within the time window \(T\). window \(=[t_0,t\). f , and the collision probability is expressed as:

[0198]

[0199] In the formula, \(R\). colision (T\). i ) represents the collision probability between the predetermined trajectory of the driverless vehicle and the predicted trajectory of the human-driven vehicle; \(\zeta(t)\) represents the time weight, which is designed as an exponential decay function and is expressed as:

[0200]

[0201] In the formula, \(\lambda\) represents the decay coefficient.

[0202] The total collision probability of the set of the predetermined trajectory of the driverless vehicle and the predicted trajectories of the human-driven vehicle is obtained by comprehensively considering the confidence and collision probability of each predicted trajectory, and is expressed as:

[0203]

[0204] In the formula, \(K\) represents the total number of predicted trajectories of the \(i\)th human-driven vehicle.

[0205] When the total collision risk probability of the predicted full trajectory sets of driverless vehicles and human-driven vehicles exceeds a set threshold, i.e., R colision,total >R safe , the occupancy prediction trajectory set within the future time t ∈ T window is mapped into the spatio-temporal resource field to form a spatio-temporal risk cost field. This spatio-temporal risk cost field is used to guide subsequent trajectory search, optimize trajectory planning, and avoid potential collision risks.

[0206] The construction of the spatio-temporal risk cost field is as follows:

[0207] Quantify the risk value of each spatio-temporal point in the spatio-temporal resource field. For each moment t and each spatial position (x, y), define its basic risk value which is expressed as:

[0208]

[0209] Traverse the involved spatio-temporal points to obtain the spatio-temporal risk cost field. This spatio-temporal risk cost field reflects the collision risk level at different spatial positions at the current moment by comprehensively considering the occupancy probability of each trajectory point and its distribution characteristics in the spatio-temporal resource field.

[0210] Based on the collision risk estimation obtained by cloud computing, the collaborative replanning system provides real-time risk warnings for human-driven vehicles and proposes specific adjustment suggestions (such as deceleration, acceleration, path correction) to ensure that human-driven vehicles can avoid risks in time. On the other hand, the system redistributes the spatio-temporal resources of human driving based on the adjustment suggestions, and at the same time readjusts the priorities of affected driverless vehicles according to the collision probability of the vehicle group spatio-temporal risk field. Its allocation logic is: the priority is proportional to the collision risk. The cloud transmits the adjusted priorities and the dynamic risk field to the vehicle end, as the heuristic information for the subsequent spatio-temporal hybrid A* search algorithm, to optimize the smoothness and safety of the trajectory by comprehensively considering path smoothness, speed change, direction adjustment cost, and collision risk, and to achieve the elimination of trajectory conflicts and collaborative optimization in the hybrid vehicle group scenario.

[0211] When the system determines through the predicted trajectory that a human-driven vehicle may encroach on the spatio-temporal resource occupancy area of a driverless vehicle, it will provide adjustment suggestions for the human-driven vehicle:

[0212] First, the system identifies human-driven vehicles that may collide based on the estimated collision probability. By comparing their pre-assigned spatio-temporal resources with the predicted occupied spatio-temporal resources, it further determines specific risk behaviors and corresponding adjustment suggestions: when the vehicle is speeding and approaching a high-risk area, it prompts the driver to decelerate; in the case of low-speed driving or long-term idling, it advises the driver to increase the speed; when the vehicle's path deviates from the recommended trajectory and enters a high-risk area, it provides left or right deviation correction suggestions to avoid potential dangers. The early warning is transmitted to in-vehicle devices through real-time communication and presented in the form of a voice warning.

[0213] Step S5: Introduce the risk cost of the extended step size in the spatio-temporal hybrid A* algorithm to evaluate the cumulative collision risk of the future trajectory of the driverless vehicle from the current node to the end point in spatio-temporal resources, and generate a collision-free smooth trajectory for the driverless vehicle.

[0214] The spatio-temporal hybrid A* algorithm represents the expanded state node as: s(x, y, θ, t), where: x and y respectively represent the lateral coordinate and longitudinal coordinate of the driverless vehicle; θ is the heading angle of the driverless vehicle; t is the time.

[0215] For each exploration, a position update that satisfies its kinematic constraints is generated as follows:

[0216] Δd = v n+1 ·ΔT;

[0217]

[0218] θ n+1 = θ n +Δθ;

[0219] x n+1 = x n + R r ·(sinθ n+1 - sinθ n );

[0220] y n+1 = y n + R r ·(cosθ n - cosθ n+1 );

[0221] t n+1 = t n +ΔT;

[0222] In the formula, ΔT represents the time resolution; Δd represents the change in the driving distance of the driverless vehicle; v n+1 represents the speed of the driverless vehicle at the n+1 node; αθ represents the change in the steering angle of the driverless vehicle; δ jdenotes the front wheel steering angle of the driverless vehicle; L denotes the wheelbase of the driverless vehicle; θ n+1 , θ n respectively denote the heading angles of the driverless vehicle at the n+1 node and the n node; x n+1 , y n+1 respectively denote the lateral coordinates of the driverless vehicle at the n+1 node and the n node; y n+1 , y n respectively denote the longitudinal coordinates of the driverless vehicle at the n+1 node and the n node; R r denotes the turning radius of the driverless vehicle; where:

[0223]

[0224] To ensure the rationality of the driverless vehicle control, constraints on acceleration, steering angle and speed are set, expressed as:

[0225] a min ≤ a i ≤ a max ;

[0226] -δ max ≤ δ j ≤ δ max ;

[0227] 0 ≤ v n+1 ≤ v max .

[0228] To avoid obstacles, the present invention introduces a risk cost c(n) of an extended step length in the spatio-temporal hybrid A* algorithm, which is used to evaluate the cumulative collision risk of the future trajectory from the current node to the end point in spatio-temporal resources. This future extended trajectory is assumed to be a uniform Cartesian path from the current point to the end point, and the spatio-temporal heuristic cost of the current node is:

[0229] c(n) = ∑ζ(t)Colision(T i , T ego , t);

[0230] Therefore, the total cost function f(n) of the spatio-temporal hybrid A* based on the dynamic risk field is expressed as:

[0231] f(n) = h(n) + g(n) + c(n);

[0232] In the formula, h(n) represents the expected future cost from the current node to the target node; g(n) represents the cumulative historical cost from the starting node to the current node; c(n) represents the spatio-temporal heuristic cost of the current node, where:

[0233] h(n) = ||s n - s G ||2 = |xn -x G |+|y n -y G |+|t n -t G |;

[0234] Wherein, s n and s G respectively represent the spatio-temporal coordinates of the current node and the target node, s n =(x n , y n , t n ), s G =(x G , y G , t G ).

[0235] h(n) consists of two parts. The first part |x n -x G |+|y n -y G | considers the spatial distance difference between the current node s n and the target node s G ; The second part |t n -t G | considers the time difference between the current node s n and the target node s G to encourage the vehicle to arrive at the destination on time according to the planned time.

[0236] g(n)=g(n - 1)+Δd+w v ·|v n -v n-1 |+w ref ·|v n -v ref |+w δ ·|θ n -θ n-1 |;

[0237] g(n) consists of five parts: The first part g(n - 1) represents the cost inherited from the previous node; The second part Δd represents the cost of distance expansion, measuring the geographical distance of the path; The third part w v ·|v n -v n-1 | represents the cost of speed change, used to reduce frequent speed adjustments in the path; The fourth part w ref ·|v n -v ref | represents the cost of maintaining the reference speed, used to ensure that the vehicle maintains the reference speed in the path, improving the form efficiency and safety; The fifth part w δ·|θ n -θ n-1 | represents the cost of direction change, which is used to reduce frequent steering changes and ensure a smooth path.

[0238] After predicting the rough trajectory of the driverless vehicle based on the dynamic risk field using the spatio-temporal hybrid A* algorithm, the path and speed are smoothed to obtain a more comfortable trajectory, where:

[0239] Path optimization objective function F p is expressed as:

[0240] minF p = ω s · f s (X) + ω r · f r (X) + ω l · f l (X);

[0241] In the formula, f s (X) represents the path smoothing cost, which is used to reduce sharp turns or irregular changes in the trajectory to improve driving smoothness; f r (X) represents the path fitting original trajectory cost, which is used to make the generated smooth trajectory as close as possible to the original trajectory, thus maintaining the rationality and accuracy of the trajectory; f l (X) represents the path uniformity cost, which is used to optimize the overall balance of the trajectory, making the acceleration change smoothly during driving and avoiding frequent acceleration or deceleration; ω s , ω r , ω l respectively represent the weight coefficients of f s (X), f r (X), f l (X);

[0242] Speed optimization objective function F v is expressed as:

[0243] minF v = ω v · f v (S) + ω a · f a (S) + ω jerk · f jerk (S);

[0244] In the formula, f v (S) represents the speed deviation cost, which is used to measure the difference between the actual speed of the vehicle and the reference speed, thereby reducing speed fluctuations and improving driving stability, f v (S) = ∑(v k - v ref )2 ; f a (S) represents the acceleration change cost, which is used to reduce the drastic change of vehicle acceleration and ensure the smoothness during driving. f jerk (S) represents the jerk change cost, which is used to reduce the fluctuation of the acceleration change rate and avoid causing an uncomfortable riding experience, f jerk (S) = ∑(a k+1 -a k ) 2 .

[0245] The embodiments of the present invention have been described above in conjunction with the accompanying drawings. However, the present invention is not limited to the above specific embodiments. The above specific embodiments are merely illustrative rather than restrictive. Under the inspiration of the present invention, those of ordinary skill in the art can also make many forms without departing from the spirit of the present invention and the scope protected by the claims, and all of them belong to the protection scope of the present invention.

Claims

1. A path coordination planning method for a mixed vehicle group conflict area, characterized in that: The mixed vehicle group includes manually driven vehicles and unmanned vehicles, and the path coordination planning method for the conflict area of ​​the mixed vehicle group includes the following steps: Step S1, sampling the speed of the manually driven vehicle and the deviation path distance relative to the road reference line to obtain the driver's virtual following target point, modeling the driver's driving behavior, and generating a sampling trajectory of the manually driven vehicle under random driving behavior; Step S2, predicting the short-term trajectory of the manually driven vehicle as a reference trajectory based on a forward deduction method of vehicle dynamics; Step S3, calculating the smoothness cost of the sampled trajectory and the similarity cost between the sampled trajectory and the reference trajectory, combining the smoothness cost and the similarity cost to form a total cost, evaluating the confidence of each sampled trajectory, and determining the trajectory prediction result of the manually driven vehicle based on the confidence evaluation result; Step S4, discretizing the spatiotemporal resources in the conflict area, calculating the probability of collision between the unmanned vehicle and the manually driven vehicle under the predetermined trajectory according to the trajectory prediction result of the manually driven vehicle, providing real-time risk warning for the manually driven vehicle based on the calculated collision risk estimate, and making adjustment suggestions; at the same time, readjusting the priority of the affected unmanned vehicles to eliminate the trajectory conflict between the unmanned vehicle and the manually driven vehicle; Step S5, introducing the risk cost of the extended step length into the spatiotemporal hybrid A-star algorithm, evaluating the cumulative collision risk of the future trajectory from the current node to the end point of the unmanned vehicle in the spatiotemporal resources, and generating a collision-free smooth trajectory for the unmanned vehicle.

2. The path coordination planning method for a mixed vehicle group conflict area according to claim 1, characterized in that: Step S1 specifically includes the following steps: The reference speed obtained by the roadside unit is used to set the floating value to generate the sampled speed of the manually driven vehicle; a discrete deviation set near 0 is selected as the sampled lateral deviation of the manually driven vehicle; Generate the trajectory of the manually driven vehicle in the Frenet coordinate system; Based on the road reference line, the trajectory of the manually driven vehicle in the Frenet coordinate system is converted to the Cartesian coordinate system. The conversion relationship is expressed as: In the formula, x ref ,y ref The points represent the horizontal and vertical coordinates of the projection point of the sampling trajectory point i of the manually driven vehicle onto the road reference line in the Cartesian coordinate system; ψ s represents the sampling time t s , the heading angle of the manually driven vehicle i; Respectively represent the sampling time t s The horizontal and vertical coordinates of the sampling trajectory point i of the manually driven vehicle in the Cartesian coordinate system; The velocity component of the manually driven vehicle i in the Cartesian coordinate system is obtained by combining the velocity and angular velocity in the Frenet coordinate system: In the formula, Respectively represent the sampling time t s When , the velocity components of the manually driven vehicle i in the x-axis and y-axis directions of the Cartesian coordinate system; s s '(t s ), d s '(t s ) represent the sampling time t s When , the first-order differential of the horizontal and vertical coordinates of the sampling trajectory point of the manually driven vehicle i in the Frenet coordinate system; θ s represents the sampling time t s When , the yaw angle of the manually driven vehicle i in the Cartesian coordinate system is expressed as: Where κ represents the curvature of the road reference line; Sampling time t s When the speed of the manually driven vehicle i in the Cartesian coordinate system is It is expressed as: After obtaining the terminal sampling states of the x-axis and y-axis directions in the Cartesian coordinate system, the trajectories of the artificially driven vehicle i in the x-axis and y-axis directions of the Cartesian coordinate system are generated using a quintic polynomial. The generation process is expressed as follows: In the formula, Respectively represent the sampling time t s When , the trajectory of the manually driven vehicle i in the x-axis and y-axis directions of the Cartesian coordinate system; a0, a1, …, a5 and b0, b1, …, b5 all represent polynomial coefficients.

3. The path coordination planning method for a mixed vehicle group conflict area according to claim 2, characterized in that: Sampling trajectory of artificially driven vehicle i generated by quintic polynomial It is expressed as: In the formula, Respectively represent the sampling time t s The lateral coordinate, longitudinal coordinate, velocity, acceleration and yaw rate of the manually driven vehicle i in the Cartesian coordinate system at .

4. The path coordination planning method for a mixed vehicle group conflict area according to claim 3, characterized in that: Step S2 specifically includes the following steps: The short-term trajectory of the artificially driven vehicle is obtained using the constant turning rate and acceleration model. The state space and state transition of the system are expressed as: In the formula, Indicates t c The state space of the manually driven vehicle i at time instant; Respectively represent t c The lateral coordinate, longitudinal coordinate, velocity, acceleration and yaw rate of the manually driven vehicle i in the Cartesian coordinate system at the moment; Denotes Δt c The state space of the manually driven vehicle i after the operation cycle; T represents the transposed matrix; It represents the state transfer amount, which is obtained through CTRA kinematics and is expressed as: In the formula, Indicates t c The yaw angle of the manually driven vehicle i at time i; Indicates t c The yaw rate of the manually driven vehicle i at time i; The reference trajectory of the manually driven vehicle i based on vehicle dynamics prediction is expressed as:

5. The path coordination planning method for a mixed vehicle group conflict area according to claim 4, characterized in that: The trajectory smoothness cost includes the lateral smoothness cost and the longitudinal smoothness cost, where the lateral smoothness cost J lat The trajectory smoothness is achieved by minimizing the lateral acceleration and lateral jerk, and the longitudinal smoothness cost J lon represents the constraint on the rate of change of longitudinal velocity; The lateral smoothness cost J of the manually driven vehicle i lat It is expressed as: In the formula, represents the lateral acceleration of the manually driven vehicle i in the Cartesian coordinate system, κ; It represents the lateral acceleration of the manually driven vehicle i in the Cartesian coordinate system, which is the rate of change of the lateral acceleration with time. The derivative is obtained, expressed as: Longitudinal smoothness cost J lon It is expressed as: In discrete time, It is calculated by the velocity difference of adjacent sampling trajectory points and is expressed as: In the formula, They represent the speed of the manually driven vehicle i in the Cartesian coordinate system when it is located at sampling trajectory point j and j+1; Δt s represents the time interval between sampling trajectory points j and j+1; Then the trajectory smoothness cost of the manually driven vehicle i is It is expressed as: In the formula, w lat 、w lon Respectively represent J lat and J lon The weight coefficient of .

6. The path coordination planning method for a mixed vehicle group conflict area according to claim 5, characterized in that: The similarity cost between the reference trajectory and the sampled trajectory includes position similarity cost, velocity similarity cost, acceleration similarity cost and angular velocity similarity cost; among them, the position similarity cost uses the Euclidean distance to evaluate the spatial deviation of the trajectory points on the reference trajectory and the sampled trajectory, the velocity similarity cost compares the mean square error of the velocity features of the sampled trajectory and the reference trajectory, the acceleration similarity cost evaluates the mean square error of the acceleration features, and the angular velocity similarity cost evaluates the mean square error of the angular velocity features; The position similarity cost between the kth sampled trajectory of the manually driven vehicle i and the reference trajectory It is expressed as: Where m represents the trajectory point on the kth sampling trajectory of the manually driven vehicle i; N represents the number of trajectory points on the kth sampling trajectory of the manually driven vehicle i; The speed similarity cost between the kth sampled trajectory of the manually driven vehicle i and the reference trajectory It is expressed as: Acceleration similarity cost between the kth sampled trajectory of the manually driven vehicle i and the reference trajectory It is expressed as: Angular velocity similarity cost between the kth sampled trajectory of the manually driven vehicle i and the reference trajectory It is expressed as: Then the similarity cost between the kth sampled trajectory of the manually driven vehicle i and the reference trajectory is It is expressed as:

7. The path coordination planning method for a mixed vehicle group conflict area according to claim 6, characterized in that: The total cost of the kth sampled trajectory of the manually driven vehicle i It is expressed as: The confidence of the kth sampling trajectory of the manually driven vehicle i is expressed as: In the formula, represents the probability of the kth sampling trajectory of the manually driven vehicle i.

8. The path coordination planning method for a mixed vehicle group conflict area according to claim 7, characterized in that: Whether there is a collision risk between the unmanned vehicle and the manually driven vehicle is determined by detecting the Euclidean distance between the predicted trajectory of the unmanned vehicle and the predicted trajectory of the manually driven vehicle. The collision condition is determined as follows: window ,If the Euclidean distance between the predetermined trajectory point of the unmanned vehicle and the trajectory point predicted by the manual vehicle is less than the set safety distance threshold d safe , it is considered that the trajectories overlap and there is a risk of collision: In the formula, Colision(T i ,T ego ,t) represents the collision risk between the unmanned vehicle and the manual vehicle at time t. The value is 1, indicating that there is a collision risk between the unmanned vehicle and the manual vehicle, and the value is 0, indicating that there is no collision risk between the unmanned vehicle and the manual vehicle; T ego represents the planned trajectory of the unmanned vehicle; x ego (t), y ego (t) represent the horizontal and vertical coordinates of the predetermined trajectory point of the unmanned vehicle at time t, respectively; The overall collision risk estimation of the scheduled trajectory of the unmanned vehicle and the predicted trajectory of the manually driven vehicle requires traversing all trajectory points on the trajectory, and the time window T window =[t0,t f ] is weighted summation of the risk values ​​within , and the collision probability is expressed as: In the formula, R colision (T i ) represents the collision probability between the predetermined trajectory of the unmanned vehicle and the predicted trajectory of the manually driven vehicle; ζ(t) represents the time weight, which is designed as an exponential decay function and is expressed as: Where λ represents the attenuation coefficient; The total collision probability of the set of predicted trajectories of the unmanned vehicle and the manually driven vehicle is obtained by comprehensively considering the confidence and collision probability of each predicted trajectory, which is expressed as: Where K represents the total number of predicted trajectories of manually driven vehicle i; When the total collision risk probability of the unmanned vehicle and the full trajectory set predicted by the human-driven vehicle exceeds the set threshold, R colision,total >R safe , the future time t∈T window The occupancy prediction trajectory set within is mapped into the spatiotemporal resource field to form the spatiotemporal risk cost field.

9. The path coordination planning method for a mixed vehicle group conflict area according to claim 8, characterized in that: The construction of the space-time risk cost field is as follows: Quantify the risk value of each space-time point in the space-time resource field, and define its basic risk value for each time t and each spatial position (x, y). It is expressed as: The space-time risk cost field is obtained by traversing the involved space-time points. The space-time risk cost field reflects the collision risk level of different spatial positions at the current moment by comprehensively considering the occupancy probability of each trajectory point and its distribution characteristics in the space-time resource field.

10. The path coordination planning method for a mixed vehicle group conflict area according to claim 9, characterized in that: The space-time hybrid A-star algorithm represents the extended state node as: s(x, y, θ, t), where: x, y represent the lateral coordinate and longitudinal coordinate of the unmanned vehicle respectively; θ is the heading angle of the unmanned vehicle; t is the time; For each exploration, a position update that satisfies its kinematic constraints is generated as follows: Δd=v n+1 ·ΔT; i n+1 =θ n +Δθ; x n+1 =x n +R r ·( s inθ n+1 -sinθ n ); y n+1 =y n +R r ·(cosθ n -cosθ n+1 ); t n+1 =t n +ΔT; In the formula, ΔT represents the time resolution; Δd represents the change in the driving distance of the unmanned vehicle; v n+1 represents the speed of the unmanned vehicle at the n+1 node; Δθ represents the change in the steering angle of the unmanned vehicle; δ j represents the front wheel turning angle of the unmanned vehicle; L represents the wheelbase of the unmanned vehicle; θ n+1 ,θ n Respectively represent the heading angle of the unmanned vehicle at the n+1 node and the n node; x n+1 ,y n+1 Respectively represent the lateral coordinates of the unmanned vehicle at the n+1 node and the n node; y n+1 ,y n Respectively represent the longitudinal coordinates of the unmanned vehicle at the n+1 node and the n node; R r represents the turning radius of the driverless vehicle; where: In order to ensure the rationality of the unmanned vehicle control, the constraints of acceleration, steering angle and speed are set, which can be expressed as: a min ≤a i ≤a max ; -d max ≤δ j ≤δ max ; 0≤v n+1 ≤v max 。 The risk cost c(n) of the extension step is introduced in the spatiotemporal hybrid A-star algorithm to evaluate the cumulative collision risk of the future trajectory from the current node to the end point in the spatiotemporal resources. The future extension trajectory is assumed to be a uniform Cartesian path from the current point to the end point. The spatiotemporal heuristic cost of the current node is: c(n)=∑ζ(t)Colision(T i ,T ego ,t); Therefore, the total cost function f(n) of the space-time hybrid A star based on the dynamic risk field is expressed as: f(n)=h(n)+g(n)+c(n); In the formula, h(n) represents the expected future cost from the current node to the target node; g(n) represents the cumulative historical cost from the starting node to the current node; c(n) represents the spatiotemporal heuristic cost of the current node, where: h(n)=||s n -s G ||2=|x n -x G |+|y n -y G |+|t n -t G |; In the formula, s n 、s G Represent the spatiotemporal coordinates of the current node and the target node, s n =(x n ,y n ,t n ), s G =(x G ,y G ,t G ); g(n)=g(n-1)+Δd+w v ·|v n -v n-1 |+in ref ·|v n -v ref |+in δ ·|θ n -θ n-1 |; After the rough trajectory of the unmanned vehicle based on the dynamic risk field is predicted based on the spatiotemporal hybrid A-star algorithm, the path and speed are smoothed to obtain a more comfortable trajectory, where: Path optimization objective function F p It is expressed as: minF p =ω s ·f s (X)+ω r ·f r (X)+ω l ·f l (X); In the formula, f s (X) represents the path smoothing cost, which is used to reduce sharp turns or irregular changes in the trajectory to improve driving smoothness; f r (X) represents the cost of the path fitting the original trajectory, which is used to make the generated smooth trajectory as close to the original trajectory as possible, thereby maintaining the rationality and accuracy of the trajectory; f l (X) represents the path uniformity cost, which is used to optimize the overall balance of the trajectory so that the acceleration changes smoothly during driving and avoid frequent acceleration or deceleration; ω s ,ω r ,ω l Respectively represent f s (X), f r (X), f l (X) weight coefficient; Speed ​​optimization objective function F v It is expressed as: minF v =ω v ·f v (S)+ω a ·f a (S)+ω jerk ·f jerk (S); In the formula, f v (S) represents the speed deviation cost, f v (S) = ∑(v k -v ref ) 2 ;f a (S) represents the acceleration change cost, f jerk (S) represents the acceleration change cost, f jerk (S) = ∑(a k+1 -a k ) 2 .

Images