A path coordination planning method for mixed vehicle conflict areas

Abstract

The application provides a path coordination planning method for a mixed vehicle group conflict area, and belongs to the technical field of vehicle path planning. The application obtains a virtual following target point of a driver by sampling the speed of an artificial driving vehicle and the deviation path distance relative to a road reference line to generate a sampling trajectory, predicts the driving behavior of the driver in an unstructured road environment, and combines a forward deduction method of vehicle dynamics to predict a short-time trajectory of the artificial driving 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. In addition, in combination with the information of a time-space risk field, a time-space hybrid A-star algorithm based on a dynamic risk field is used to dynamically adjust the priority and driving trajectory of an unmanned driving vehicle, so as to realize efficient dynamic redistribution of time-space resources in a conflict area scene and reduce the collision risk of vehicles in the conflict area. The application can significantly improve the stability of multi-vehicle coordination operation in a mixed scene.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of vehicle path planning, and particularly relates to a path coordination planning method for a conflict area of a hybrid vehicle group. BACKGROUND

[0002] Multi-vehicle coordination planning technology refers to, in a scene such as port and mine operation, coordinating the motion trajectories of multiple vehicles to ensure that they safely and efficiently complete tasks in conflict areas such as intersections.

[0003] In actual application scenarios, for the purpose of gradual automation, part of the unmanned vehicles and traditional manually driven vehicles are often introduced to work cooperatively, forming a hybrid vehicle group of manually driven vehicles and unmanned vehicles. In the multi-vehicle coordination work of the hybrid vehicle group, not only the efficiency of the coordination planning needs to be concerned, but also the running status of the vehicle group needs to be monitored in real time. The driving behavior of the driver in the manually driven vehicle has certain randomness, which may destroy the result of the coordination planning and increase the risk of vehicle collision. Moreover, in unstructured scenes 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, so that the trajectory of the manually driven vehicle is difficult to accurately predict, and it is impossible to effectively plan a reasonable path for the hybrid vehicle.

[0004] Therefore, it is necessary to provide a path coordination planning method for a conflict area of a hybrid vehicle group to solve the above problems. SUMMARY

[0005] The present application provides a path coordination planning method for a conflict area of a hybrid vehicle group, which monitors the running status of the hybrid vehicle group based on a cloud platform, predicts the random driving behavior of the manually driven vehicle, and quantitatively evaluates the potential risks caused by the random driving behavior. On this basis, the priority and driving trajectory of the unmanned vehicle are dynamically adjusted by using a time-space hybrid A-star algorithm based on a dynamic risk field, the time-space resources in the conflict area scene are efficiently and dynamically redistributed, and the collision risk of the vehicles in the conflict area is reduced, which can effectively solve at least one technical problem involved in the background technology.

[0006] To solve the above technical problems, the present application is implemented as follows:

[0007] A path coordination planning method for a conflict area of a hybrid vehicle group, the hybrid vehicle group comprising manually driven vehicles and unmanned vehicles, the path coordination planning method for the conflict area of the hybrid vehicle group comprising the following steps:

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

[0009] Step S2: Predict the short-term trajectory of the manually driven vehicle as a reference trajectory using the forward extrapolation method based on 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. Combine the smoothness cost and the similarity cost to form the total cost. Evaluate the confidence of each sampled trajectory. Determine the trajectory prediction result of the artificial driving vehicle based on the confidence evaluation result.

[0011] Step S4: Discretize the spatiotemporal resources of the conflict area, calculate the collision probability between the autonomous vehicle and the human-driven vehicle under the predetermined trajectory based on the trajectory prediction results of the human-driven vehicle, provide real-time risk warning for the human-driven vehicle based on the calculated collision risk estimate, and propose adjustment suggestions; at the same time, readjust the priority of the affected autonomous vehicles to eliminate the trajectory conflict between the autonomous vehicle and the human-driven vehicle.

[0012] Step S5 introduces the risk cost of expanding the step size into the spatiotemporal hybrid A-Star algorithm, evaluates the cumulative collision risk of the future trajectory from the current node to the endpoint of the autonomous vehicle in the spatiotemporal resources, and generates a collision-free smooth trajectory for the autonomous vehicle.

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

[0014] The reference speed obtained from the roadside unit is used to set a floating value to generate the sampling speed of the manually driven vehicle; the discrete deviation set near 0 is selected as the sampling lateral deviation of the manually driven vehicle; the trajectory of the manually driven vehicle in the Frenet coordinate system is generated.

[0015] Using the road reference line as a reference, the trajectory of the manually driven vehicle in the Frenet coordinate system is transformed to the Cartesian coordinate system. The transformation relationship is expressed as follows:

[0016]

[0017] In the formula, x ref y ref The subscript represents the horizontal and vertical coordinates of the projection points of the manually driven vehicle i's sampled trajectory points onto the road reference line in the Cartesian coordinate system; ψ s Indicates sampling time t s At that time, the heading angle of the manually driven vehicle i; They represent sampling time t respectively. s The x and y coordinates of the sampled trajectory points of the manually driven vehicle i in the Cartesian coordinate system;

[0018] The velocity component of the manually driven vehicle i in the Cartesian coordinate system is obtained by the velocity and angular velocity in the Frenet coordinate system:

[0019]

[0020] In the formula, respectively represent the sampling time t s , the velocity component of the manually driven vehicle i in the x-axis and y-axis direction of the Cartesian coordinate system; s s '(t s ), d s '(t s ) respectively represent the first-order differential of the sampling trajectory point of the manually driven vehicle i in the Frenet coordinate system at the sampling time t s ; θ s represents the yaw angle of the manually driven vehicle i in the Cartesian coordinate system at the sampling time t s , which is expressed as:

[0021]

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

[0023] The velocity of the manually driven vehicle i in the Cartesian coordinate system at the sampling time t s is expressed as:

[0024]

[0025] After obtaining the termination sampling state in the x-axis and y-axis direction of the Cartesian coordinate system, a quintic polynomial is used to generate the trajectory of the manually driven vehicle i in the x-axis and y-axis direction of the Cartesian coordinate system, and the generation process is respectively expressed as:

[0026]

[0027]

[0028] In the formula, respectively represent the trajectory of the manually driven vehicle i in the x-axis and y-axis direction 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 of the manually driven vehicle i generated by the quintic polynomial is expressed as:

[0030]

[0031] In the formula,​​ denote the lateral coordinate, longitudinal coordinate, velocity, acceleration and yaw rate of the manually driven vehicle i in the Cartesian coordinate system at time t s denote the lateral coordinate, longitudinal coordinate, velocity, acceleration and yaw rate of the manually driven vehicle i in the Cartesian coordinate system at time t

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

[0033] The short-term trajectory of the manually driven vehicle is obtained using a constant turning rate and acceleration model, the state space of the system and the state transition are represented as:

[0034]

[0035] wherein, denote the state space of the manually driven vehicle i at time t c denote the lateral coordinate, longitudinal coordinate, velocity, acceleration and yaw rate of the manually driven vehicle i in the Cartesian coordinate system at time t c denote the state space of the manually driven vehicle i after a running period Δt c denote the state space of the manually driven vehicle i after a running period Δt denote the state transition, which is obtained through CTRA kinematics, and is represented as:

[0036]

[0037] wherein, denote the yaw angle of the manually driven vehicle i at time t c denote the yaw rate of the manually driven vehicle i at time t c

[0038] The reference trajectory of the manually driven vehicle i based on vehicle dynamics prediction is represented as:

[0039]

[0040] As a preferred improvement, the trajectory smoothness cost comprises a lateral smoothness cost and a longitudinal smoothness cost, wherein the lateral smoothness cost J lat The smoothness of the trajectory is achieved through minimization of the lateral acceleration and lateral jerk, and the longitudinal smoothness cost J lon denote the constraint on the longitudinal velocity change rate;

[0041] The lateral smoothness cost J lat of the manually driven vehicle i is represented as:

[0042]

[0043] wherein,​​​​ 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, is the rate of change of the lateral acceleration with respect to time, and is obtained by derivation of represents:

[0044]

[0045] longitudinal smoothness cost J lon represents:

[0046]

[0047] in discrete time, is obtained by difference calculation of the velocities of adjacent sampling trajectory points, and represents:

[0048]

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

[0050] the trajectory smoothness cost of the manually driven vehicle i represents:

[0051]

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

[0053] As a preferred improvement, the similarity cost of the reference trajectory and the sampling trajectory includes a position similarity cost, a velocity similarity cost, an acceleration similarity cost, and an angular velocity similarity cost; wherein the position similarity cost uses the Euclidean distance to evaluate the spatial deviation of the trajectory points on the reference trajectory and the sampling trajectory, the velocity similarity cost compares the mean square error of the velocity features of the sampling 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;

[0054] the position similarity cost of the kth sampling trajectory of the manually driven vehicle i and the reference trajectory represents:

[0055]

[0056] In the formula, 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;

[0057] The velocity similarity cost of the kth sampling trajectory of the manually driven vehicle i and the reference trajectory is expressed as:

[0058]

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

[0060]

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

[0062]

[0063] The similarity cost of the kth 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 kth sampling trajectory of the manually driven vehicle i is expressed as:

[0066]

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

[0068]

[0069] In the formula, represents the probability of the kth sampling trajectory of the manually driven vehicle i.

[0070] As a preferred improvement, whether the unmanned vehicle and the manually driven vehicle have a collision risk is judged by detecting the Euclidean distance between the predetermined trajectory of the unmanned vehicle and the predicted trajectory of the manually driven vehicle, and the collision condition is judged as follows: in the prediction time window t∈T window , if the Euclidean distance between the predetermined trajectory point of the unmanned 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 the trajectory overlap occurs, and thus there is a collision risk:

[0071]

[0072] where Collision(T i ,T ego ,t) represents the collision risk of the AV and the HV at time t, taking value 1 if the AV and the HV have collision risk, and 0 otherwise; T ego represents the planned trajectory of the AV; x ego (t), y ego (t) represent the horizontal and vertical coordinates of the planned trajectory point of the AV at time t, respectively;

[0073] The overall collision risk estimation of the AV planned trajectory and the HV predicted trajectories needs to traverse all trajectory points on the trajectories, and is weightedly summed by the risk values within the time window T window = [t0, t f ], and the collision probability is represented as:

[0074]

[0075] where R colision (T i ) represents the collision probability of the AV planned trajectory and the HV predicted trajectories; ζ(t) represents the time weight, designed as an exponential decay function, and is represented as:

[0076]

[0077] where λ represents the decay coefficient;

[0078] The overall collision probability of the AV planned trajectory and the HV predicted trajectory set is obtained by comprehensively considering the confidence and collision probability of each predicted trajectory, and is represented as:

[0079]

[0080] where K represents the total number of the HV i predicted trajectories;

[0081] When the overall collision risk probability of the AV and the HV predicted trajectory set exceeds the set threshold, i.e. R colision,total > R safe , the occupancy predicted trajectory set within the future time t ∈ T window is mapped into the space-time resource field to form a space-time risk cost field.

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

[0083] Quantifying the risk value of each space-time point in the space-time resource field, for each time t, each spatial position (x, y), define its basic risk value is expressed as:

[0084]

[0085] The space-time risk cost field is obtained by traversing the space-time points involved, which reflects the collision risk level of different spatial positions at the current time by comprehensively considering the occupancy probability of each trajectory point and its distribution characteristics in the space-time resource field.

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

[0087] For each exploration, a position update that satisfies its kinematic constraint is generated 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 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 steering angle of the unmanned vehicle; L represents the wheelbase of the unmanned vehicle; θ n+1 , θ n respectively represent the heading angles of the unmanned vehicle at the n+1 node and the n node; xn+1 , y n+1 represent the lateral coordinates of the autonomous vehicle at the n+1th node and the nth node, respectively; n+1 , y n represent the longitudinal coordinates of the autonomous vehicle at the n+1th node and the nth node, respectively; R r represents the turning radius of the autonomous vehicle; wherein:

[0095]

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

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

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

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

[0100] In the space-time hybrid A-star algorithm, the risk cost c(n) of the expansion step is introduced to evaluate the cumulative collision risk of the future trajectory from the current node to the end point in the space-time resource, and the future expansion trajectory is assumed to be a uniform speed Cartesian path from the current point to the end point. The space-time 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 space-time hybrid A-star based on the dynamic risk field is:

[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 space-time heuristic cost of the current node, wherein:

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

[0106] where s n and s G represent the spatiotemporal coordinates of the current node and the target node, respectively, s n = (x n , y n , t n ), s G = (x G , y G , t G );

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

[0108] After predicting the coarse trajectory of the unmanned vehicle based on the dynamic risk field based on the spatiotemporal hybrid A-star algorithm, the path and speed are smoothed to obtain a more comfortable trajectory, wherein:

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

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

[0111] wherein 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, so as to maintain the reasonableness 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 change is stable during driving, and frequent acceleration or deceleration is avoided; ω s , ω r , ω l represent the weight coefficients of f s (X), f r (X), and f l (X), respectively;

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

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

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

[0115] The beneficial effects of the present application are:

[0116] (1) The present application 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 to generate a sampled trajectory, predict the driving behavior of the driver in the unstructured road environment, and combine the forward deduction method of vehicle dynamics to predict the short-time 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 sampled trajectory, thereby improving the cooperation and safety between vehicles;

[0117] (2) Based on the space-time hybrid A-star algorithm, a single vehicle safe, collision-free, and real-time adaptive smooth trajectory in a complex dynamic environment is generated for the unmanned vehicle in the space-time graph, combined with the information of the space-time risk field, through real-time evaluation and risk analysis of the conflict area, the decision-making process of the unmanned vehicle trajectory planning is optimized, effectively avoiding the collision risk, and improving the driving safety and efficiency of the unmanned vehicle in complex scenarios. BRIEF DESCRIPTION OF DRAWINGS

[0118] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings, wherein:

[0119] Figure 1 A schematic diagram showing unstructured road conflict zones;

[0120] Figure 2 This is a flowchart illustrating the path collaborative planning method for a mixed vehicle group conflict area provided by the present invention. Detailed Implementation

[0121] The technical solutions of the present invention will be clearly and completely described below with reference to the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0122] like Figure 1 As shown, the work scenario involved in this invention comprises a mixed fleet of manually driven and driverless vehicles. All vehicles interact with the cloud platform in real time via an onboard communication module. The main functions of the onboard communication module include receiving instructions distributed by the cloud platform and uploading the dynamic status of the vehicles (such as position, speed, acceleration, etc.) to the cloud platform to support the cloud platform's global optimization and efficient management of the overall traffic flow.

[0123] For manually driven vehicles, the roadside unit sends a command to the manually driven vehicle to pass through the intersection. The command issued to manually driven vehicle i is represented as follows:

[0124]

[0125] In the formula, P i Indicates whether manually driven vehicle i is allowed to pass through the intersection, P i ∈{0,1}, P i =1 indicates that passage is allowed, P i =0 indicates waiting for approval; The suggested speed is calculated by the roadside unit based on current road conditions and the dynamic state of the manually driven vehicle; R i It indicates a suggested route, guiding vehicles along a designated path through intersections to avoid conflicts with the routes of other vehicles.

[0126] After receiving instructions, the driver of the manually driven vehicle follows the road reference lines in the lane scene. The cloud platform then allocates corresponding spatiotemporal resources to the manually driven vehicle according to the pre-defined task instructions, ensuring that it can safely pass through the intersection according to the planned path and time window.

[0127] After the space-time resource allocation of all the manually driven vehicles is completed, the unmanned vehicle adopts a method combining centralized decision and distributed planning to determine the space-time resource use priority of the unmanned vehicle on the cloud platform. Based on the priority, the unmanned vehicle generates its optimal driving trajectory by using an optimization search algorithm, so as to complete the passage of the intersection in a safe and efficient manner.

[0128] However, due to the randomness of the driving behavior of the driver in the manually driven vehicle, the manually driven vehicle has the risk of deviating from the predetermined trajectory or violating the instructions of the cloud platform, thereby causing the original space-time resource allocation scheme to be invalid, and adversely affecting the overall passage efficiency and safety of the intersection under the mixed traffic environment.

[0129] Please refer to Figure 2 , in order to solve the above problems, the 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 distance of the path 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 manually driven vehicle under the random driving behavior.

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

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

[0133] Based on the sampling speed and the sampling lateral deviation, the trajectory of the manually driven vehicle in the Frenet coordinate system is generated. In the sampling time window (0, t end ), at any sampling time t s , the path point coordinates of the manually driven vehicle i are represented as (s s , d s ), wherein s s = v s t s , the sampling speed v s and the sampling lateral deviation d s take different values, which can produce different sampling trajectories, and model different trajectories that can be produced under the random driving behavior.

[0134] The sampling trajectory at this time is generated based on the Frenet coordinate system, and for the convenience of subsequent calculation, it also needs to be converted to the Cartesian coordinate system, and the conversion process is specifically:

[0135] With the road reference line as the reference, the trajectory of the manually driven vehicle in the Frenet coordinate system is converted to the Cartesian coordinate system, and the conversion relationship is represented as:

[0136]

[0137] In the formula, x ref , y ref represent the horizontal and vertical coordinates of the projection point of the sampling trajectory point of the manually driven vehicle i to the road reference line in the Cartesian coordinate system; ψ s represents the heading angle of the manually driven vehicle i at the sampling time t s . respectively represent the horizontal and vertical coordinates of the sampling trajectory point of the manually driven vehicle i in the Cartesian coordinate system at the sampling time t s .

[0138] The velocity component of the manually driven vehicle i in the Cartesian coordinate system is 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 manually driven vehicle i in the Cartesian coordinate system x axis and y axis direction at the sampling time t s . s '(t s ), d s '(t s ) respectively represent 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 at the sampling time t s . s represents the yaw angle of the manually driven vehicle i in the Cartesian coordinate system at the sampling time t s , which is represented as:

[0141]

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

[0143] The velocity of the manually driven vehicle i in the Cartesian coordinate system at the sampling time t s is represented as:

[0144]

[0145] ​After obtaining the terminal sampling state of the x-axis and y-axis directions in the Cartesian coordinate system, a quintic polynomial is used to generate the trajectory of the manually driven vehicle i in the x-axis and y-axis directions in the Cartesian coordinate system, and the generation process is respectively represented as:

[0146]

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

[0148] The position, velocity and acceleration of the manually driven vehicle at the initial state and the terminal state are substituted 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 is The initial velocity is The initial acceleration is obtained by the road side unit; the terminal position at the terminal state is The velocity is obtained by the above sampling process, and the terminal acceleration is 0.

[0149] The sampling trajectory of the manually driven vehicle i generated by the quintic polynomial is represented as:

[0150]

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

[0152] In step S2, the short-time trajectory of the manually driven vehicle is predicted as a reference trajectory based on the forward deduction method of vehicle dynamics.

[0153] The present application uses CTRA (constant turning rate and acceleration model) to obtain the short-time trajectory of the manually driven vehicle i, and the state space and state transition of the system are represented as:

[0154]

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

[0156]

[0157] In the formula, represents t c The yaw angle of the manually driven vehicle i at time t; represents t c The yaw rate of the manually driven vehicle i at time t.

[0158] The reference trajectory of the manually driven vehicle i based on vehicle dynamics prediction is represented as:

[0159]

[0160] In step S3, the smoothness cost of the sampling trajectory and the similarity cost of the sampling trajectory and the reference trajectory are calculated, the total cost is combined from the smoothness cost and the similarity cost, the confidence of each sampling trajectory is evaluated, and the trajectory prediction result of the manually driven vehicle is determined based on the evaluation result of the confidence.

[0161] There can be multiple solutions in the quintic polynomial calculation process, that is, there can be multiple possible sampling trajectories. In view of the driving habit of human drivers, the smoothness of the driving trajectory is generally maintained, and therefore the trajectory smoothness cost is calculated to reflect the trend of the driver using the trajectory. The higher the trajectory smoothness cost, the greater the trend of the driver using the trajectory, which is used as a basis for determining the sampling trajectory.

[0162] The trajectory smoothness cost includes a lateral smoothness cost and a longitudinal smoothness cost. The lateral smoothness cost is achieved by minimizing the lateral acceleration and the lateral jerk to maintain the smoothness of the trajectory, and the longitudinal smoothness cost represents a constraint on the longitudinal speed change rate.

[0163] The lateral smoothness cost J of the manually driven vehicle i lat is represented as:

[0164]

[0165] 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 with respect to time, and is obtained by differentiating the lateral acceleration with respect to time The derivative is obtained, denoted as:

[0166]

[0167] The longitudinal smoothness cost J lon denoted as:

[0168]

[0169] In the discrete time, The velocity difference between adjacent sampling trajectory points is calculated, denoted as:

[0170]

[0171] In the formula, respectively represent the speed of the manually driven vehicle i in the Cartesian coordinate system when the manually driven vehicle i is located at sampling trajectory points j and j+1; Δt s denotes the time interval between sampling trajectory points j and j+1.

[0172] The trajectory smoothness cost J denoted as:

[0173]

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

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

[0176] The similarity cost of the reference trajectory and the sampling 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 sampling trajectory, the velocity similarity cost compares the mean square error of the velocity characteristics of the sampling 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 of the kth sampling trajectory of the manually driven vehicle i and the reference trajectory denoted as:

[0178]

[0179] In the formula, m represents a track point on the kth sampling track of the manually driven vehicle i; N represents the number of track points on the kth sampling track of the manually driven vehicle i;

[0180] The velocity similarity cost of the kth sampling track of the manually driven vehicle i and the reference track is represented as:

[0181]

[0182] The acceleration similarity cost of the kth sampling track of the manually driven vehicle i and the reference track is represented as:

[0183]

[0184] The angular velocity similarity cost of the kth sampling track of the manually driven vehicle i and the reference track is represented as:

[0185]

[0186] The similarity cost of the kth sampling track of the manually driven vehicle i and the reference track is represented as:

[0187]

[0188] The total cost of the kth sampling track of the manually driven vehicle i is represented as:

[0189]

[0190] The confidence of the kth sampling track of the manually driven vehicle i is represented as:

[0191]

[0192] In the formula, represents the probability of the kth sampling track of the manually driven vehicle i.

[0193] In step S4, the space-time resources of the conflict region are discretized, the collision probability of the unmanned vehicle under the predetermined track with the manually driven vehicle is calculated according to the track prediction result of the manually driven vehicle, real-time risk warning is provided for the manually driven vehicle based on the calculated collision risk estimation, and adjustment suggestions are proposed; at the same time, the priority of the unmanned vehicle affected is readjusted, and the track conflict between the unmanned vehicle and the manually driven vehicle is eliminated.

[0194] The collision risk between autonomous and manually driven vehicles is determined by detecting the Euclidean distance between the predetermined trajectory of the autonomous vehicle and the predicted trajectory of the manually driven vehicle. The collision condition is determined as follows: within the prediction time window t∈T window If the Euclidean distance between the predetermined trajectory point of the autonomous vehicle and the predicted trajectory point of the manually driven vehicle is less than the set safe distance threshold d safe If the trajectories overlap, then a collision risk exists.

[0195]

[0196] In the formula, Colision(T) i ,T ego The value ,t represents the collision risk between the autonomous vehicle and the manually driven vehicle at time t. A value of 1 indicates that there is a collision risk between the autonomous vehicle and the manually driven vehicle, and a value of 0 indicates that there is no collision risk between the autonomous vehicle and the manually driven vehicle. ego Indicates the planned trajectory of the driverless vehicle; x ego (t), y ego (t) represents the horizontal and vertical coordinates of the predetermined trajectory point of the unmanned vehicle at time t, respectively.

[0197] The overall collision risk estimation of the autonomous vehicle's predetermined trajectory and the human-driven vehicle's predicted trajectory requires traversing all trajectory points on the trajectory, determined by the time window T. window =[t0,t f The collision probability is expressed as a weighted sum of the risk values ​​within the range:

[0198]

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

[0200]

[0201] In the formula, λ represents the attenuation coefficient.

[0202] The total collision probability between the set of predetermined trajectories of autonomous vehicles and predicted trajectories of manually driven vehicles is obtained by comprehensively considering the confidence level and collision probability of each predicted trajectory, and is expressed as:

[0203]

[0204] In the formula, K represents the total number of predicted trajectories for manually driven vehicle i.

[0205] When the total collision risk probability of the full trajectory set predicted by the autonomous vehicle and the human-driven vehicle exceeds a set threshold, i.e. R colision,total >R safe The occupancy prediction trajectory set in the future time t∈T window The time-space risk cost field is formed by mapping the occupancy prediction trajectory set in the future time t∈T

[0206] The construction of the time-space risk cost field is as follows:

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

[0208]

[0209] The time-space risk cost field is obtained by traversing the involved time-space points. The time-space risk cost field reflects the collision risk level of different spatial positions at the current time by comprehensively considering the occupancy probability of each trajectory point and its distribution characteristics in the time-space resource field.

[0210] The collaborative re-planning system provides real-time risk warning for the human-driven vehicle based on the collision risk estimation obtained by cloud computing, and proposes specific adjustment suggestions (such as deceleration, acceleration, path correction), to ensure that the human-driven vehicle avoids risks in time. On the other hand, the system re-allocates the time-space resources of the human-driven vehicle based on the adjustment suggestions, and adjusts the priority of the affected autonomous vehicles according to the collision probability of the vehicle group time-space risk field. The allocation logic is: the priority is proportional to the collision risk. The cloud transmits the adjusted priority and dynamic risk field to the vehicle end as heuristic information for the subsequent time-space hybrid A-star search algorithm, which optimizes the smoothness and safety of the trajectory by considering the path smoothness, speed change, direction adjustment cost and collision risk, to realize trajectory conflict elimination and collaborative optimization in the mixed vehicle group scenario.

[0211] When the system determines that the human-driven vehicle may invade the time-space resource occupancy area of the autonomous vehicle through the predicted trajectory, it will provide adjustment suggestions for the human-driven vehicle:

[0212] Firstly, the system identifies the manually driven vehicle that may have a collision according to the collision probability estimation, and further determines the specific risk behavior and corresponding adjustment suggestion by comparing the pre-assigned space-time resource and the predicted occupied space-time resource: when the vehicle is overspeeding and approaching a high-risk area, the driver is prompted to slow down; in the case of low-speed driving or long-time idling, the driver is suggested to increase the speed; when the vehicle path deviates from the recommended trajectory and enters a high-risk area, the left or right deviation correction suggestion is provided to avoid potential danger. The early warning is delivered to the vehicle-mounted device through real-time communication in the form of voice warning.

[0213] Step S5, the risk cost of the extended step length is introduced into the space-time hybrid A-star algorithm to evaluate the cumulative collision risk of the future trajectory of the unmanned vehicle from the current node to the end point in the space-time resource, and to generate a smooth trajectory without collision for the unmanned vehicle.

[0214] The space-time hybrid A-star algorithm represents the extended state node as: s(x, y, θ, t), wherein: x and y represent the lateral coordinate and longitudinal coordinate of the unmanned vehicle respectively; θ is the heading angle of the unmanned vehicle; and t is the time.

[0215] For each exploration, the position update that satisfies the kinematic constraint 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 distance change of the unmanned vehicle; v n+1 represents the speed of the unmanned vehicle at the n+1 node; αθ represents the change of the steering angle of the unmanned vehicle; δ jθ represents the front wheel steering angle of the autonomous vehicle; L represents the wheelbase of the autonomous vehicle; n+1 θ n These represent the heading angles of the autonomous vehicle at nodes n+1 and n, respectively; x n+1 y n+1 Let y represent the lateral coordinates of the autonomous vehicle at node n+1 and node n, respectively; n+1 y n Let R represent the longitudinal coordinates of the autonomous vehicle at node n+1 and node n, respectively; r This represents the turning radius of the autonomous vehicle; where:

[0223]

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

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

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

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

[0228] To achieve obstacle avoidance, this invention introduces a risk cost c(n) for an extended step size into the spatiotemporal hybrid A* algorithm. This cost is used to evaluate the cumulative collision risk of the future trajectory from the current node to the destination in the spatiotemporal resources. This extended future trajectory is assumed to be a uniform Cartesian path from the current point to the destination. The spatiotemporal 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 spatiotemporal hybrid A-star 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 spatiotemporal 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] where s n , s G represent the space-time coordinates of the current node and the target node, respectively, 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 , encouraging the vehicle to arrive at the destination on schedule according to the planned time.

[0236] g(n) = g(n-1) + Ad + w v ·|v n -v n-1 |+w ref ·|v n -v ref |+w δ ·|0 n -0 n-1 |;

[0237] g(n) consists of five parts: the first part g(n-1) represents the cost inherited from the last node; the second part Ad 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 δ· | 0 n - 0 n-1 | represents the cost of changing direction, which is used to reduce frequent changes in steering and ensure smoothness of the path.

[0238] After predicting the coarse trajectory of the unmanned vehicle based on the dynamic risk field based on the space-time hybrid A-star algorithm, the path and speed are smoothed to obtain a more comfortable trajectory, wherein:

[0239] The path optimization objective function F p is represented as:

[0240] min F 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 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, so as to maintain 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 change is stable during driving, and frequent acceleration or deceleration is avoided; ω s , ω r , ω l respectively represent the weight coefficients of f s (X), f r (X), f l (X) ;

[0242] The speed optimization objective function F v is represented as:

[0243] min F 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, and then reduce the speed fluctuation and improve the stability of driving, f v (S) = ∑(v k -v ref )2 ;f a (S) represents the acceleration change cost, used to reduce the sharp change of vehicle acceleration, to ensure the smoothness during driving, f jerk (S) represents the jerk change cost, used to reduce the fluctuation of acceleration change rate, to avoid causing uncomfortable riding experience, jerk (S) =∑(a k+1 -a k ) 2 .

[0245] The embodiments of the present application are described above with reference to the accompanying drawings, but the present application is not limited to the specific embodiments described above, and the specific embodiments described above are only illustrative, but not restrictive, and those skilled in the art can make many forms under the inspiration of the present application without departing from the purpose of the present application and the scope protected by the claims, which all belong to the protection of the present application.

Claims

1. A method for path coordination planning of mixed fleet conflict areas, characterized in that, The mixed vehicle group includes manually driven vehicles and unmanned vehicles, and a path coordination planning method for a 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 a virtual following target point of the driver, modeling the driving behavior of the driver, and generating a sampled trajectory of the manually driven vehicle under random driving behavior; Step S2, predicting a short-time trajectory of the manually driven vehicle as a reference trajectory based on a forward deduction method of vehicle dynamics; Step S3, calculating a smoothness cost of the sampled trajectory and a similarity cost of 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 a trajectory prediction result of the manually driven vehicle based on the evaluation result of the confidence; Step S4, discretely processing the time-space resources of the conflict area, calculating the collision probability of the unmanned vehicle under a predetermined trajectory with the manually driven vehicle 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 estimation, and providing adjustment suggestions; meanwhile, adjusting the priority of the affected unmanned vehicle to eliminate the trajectory conflict between the unmanned vehicle and the manually driven vehicle; Step S5, introducing an extended step risk cost into a time-space hybrid A-star algorithm to evaluate the cumulative collision risk of the future trajectory of the current node of the unmanned vehicle to the end point in the time-space resources, and generating a smooth trajectory without collision for the unmanned vehicle.

2. The path coordination planning method for mixed fleet conflict areas according to claim 1, characterized in that, Step S1 specifically includes the following steps: A floating value is set by using the reference speed obtained by the roadside unit to generate the sampling speed of the manually driven vehicle; a discrete deviation set near 0 is selected as the sampling lateral deviation of the manually driven vehicle; The trajectory of the manually driven vehicle in the Frenet coordinate system is generated; The trajectory of the manually driven vehicle in the Frenet coordinate system is converted to the Cartesian coordinate system with the road reference line as the reference, and the conversion relationship is represented as: In the formula, x ref , y ref represent the horizontal and vertical coordinates of the projection point of the sampling trajectory point of the manually driven vehicle i on the road reference line in the Cartesian coordinate system. ψ s denotes the sampling time instant t s the heading angle of the manually driven vehicle i at the sampling time instant t denotes the sampling time instant t s the horizontal and vertical coordinates of the manually driven vehicle i in the Cartesian coordinate system at the sampling time instant t 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 first-order differential of the lateral and longitudinal coordinates of the sampling trajectory point of the manually driven vehicle i at the sampling moment t s , the velocity component 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 ) respectively represent the first-order differential of the lateral and longitudinal coordinates of the sampling trajectory point of the manually driven vehicle i at the sampling moment t s ; θ s represents the yaw angle of the manually driven vehicle i in the Cartesian coordinate system at the sampling moment t s , and is represented as: In the formula, κ represents the curvature of the road reference line; Sampling time t s The speed of the manually driven vehicle i in the Cartesian coordinate system is represented as: After obtaining the terminal sampling state of the x-axis and y-axis directions in the Cartesian coordinate system, a quintic polynomial is used to generate the trajectory of the manually driven vehicle i in the x-axis and y-axis directions in the Cartesian coordinate system, and the generation process is represented as: In the formula, respectively represent the sampling time t s At this time, 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 mixed platoon conflict zones according to claim 2, wherein, An autonomous vehicle i generates a sampled trajectory through a quintic polynomial is represented as: In the formula, respectively represent the sampling time t s When the human-driven vehicle i is in the lateral coordinate, longitudinal coordinate, speed, acceleration, and yaw rate of the Cartesian coordinate system.

4. The path coordination planning method of mixed-vehicle conflict zones according to claim 3, wherein, Step S2 specifically includes the following steps: A constant turning rate and acceleration model is used to obtain the short-term trajectory of the manually driven vehicle, and the state space and state transition of the system are represented as: wherein denotes the state space of the manually driven vehicle i at time t c denotes the state space of the manually driven vehicle i at time t denotes the lateral coordinate, the longitudinal coordinate, the velocity, the acceleration and the yaw rate of the manually driven vehicle i in the Cartesian coordinate system at time t c denotes the lateral coordinate, the longitudinal coordinate, the velocity, the acceleration and the yaw rate of the manually driven vehicle i in the Cartesian coordinate system at time t denotes the state space of the manually driven vehicle i after the running period Δt c denotes the state space of the manually driven vehicle i after the running period Δt denotes the state transition, which is derived by CTRA kinematics and is expressed as In the formula, denotes the yaw angle of the manually driven vehicle i at the time instant t c denotes the yaw rate of the manually driven vehicle i at the time instant t denotes the yaw angle of the manually driven vehicle i at the time instant t c denotes the yaw rate of the manually driven vehicle i at the time instant t The reference trajectory of the manually driven vehicle i predicted based on vehicle dynamics is represented as:

5. The path coordination planning method of mixed-vehicle conflict zones according to claim 4, wherein, The trajectory smoothness cost includes a lateral smoothness cost Jlatand a longitudinal smoothness cost Jlon, where lat The trajectory smoothness is achieved by minimization of lateral acceleration and lateral jerk, the longitudinal smoothness cost Jlonis given by lon represents a constraint on the longitudinal velocity change rate; Lateral smoothness cost J of the manually driven vehicle i lat is represented as: wherein denotes the lateral acceleration of the manually driven vehicle i in the Cartesian coordinate system, κ; denotes the lateral jerk of the manually driven vehicle i in the Cartesian coordinate system, is the rate of change of the lateral acceleration with respect to time, is obtained by derivation of denotes the lateral jerk of the manually driven vehicle i in the Cartesian coordinate system, is the rate of change of the lateral acceleration with respect to time, is obtained by derivation of Longitudinal smoothness cost J lon is represented as: In discrete time, The velocity difference between adjacent sampling trajectory points is calculated, expressed as: wherein respectively represent the velocity of the manually driven vehicle i in the Cartesian coordinate system at the sampled trajectory points j and j+1 ; Δt s denotes the time interval between the sampled trajectory points j and j+1 ; the trajectory smoothness cost of the human-driven vehicle i is represented as: In the formula, w lat w lon J lat and J lon The weighting coefficients.

6. The path coordination planning method of mixed-vehicle conflict zones according to claim 5, wherein, The similarity cost of the reference trajectory and the sampled trajectory includes the position similarity cost, the velocity similarity cost, the acceleration similarity cost, and the angular velocity similarity cost; wherein the position similarity cost evaluates the spatial deviation of the trajectory points on the reference trajectory and the sampled trajectory using the Euclidean distance, 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; Position similarity cost of the ith manually driven vehicle i-th sample trajectory and the reference trajectory is represented as: In the formula, 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; Cost of velocity similarity between the ith sample trajectory of the kth manually driven vehicle and the reference trajectory is represented as: Artificially driven vehicle i-th k-th sample trajectory and reference trajectory acceleration similarity cost is represented as: Artificially driven vehicle i kth sample trajectory and reference trajectory angular velocity similarity cost is represented as: a similarity cost of the kth sample trajectory of the manually driven vehicle i and the reference trajectory is represented as:

7. The path coordination planning method of mixed-vehicle conflict zones according to claim 6, wherein, total cost of the ith manually driven vehicle i for the kth sampled trajectory is represented as: The confidence of the kth sampling trajectory of the manually driven vehicle i is represented as: In the formula, denotes the probability of the kth sample trajectory of the manually driven vehicle i.

8. The path coordination planning method for mixed platoon conflict zones according to claim 7, wherein, Whether the collision risk exists between the unmanned vehicle and the manually driven vehicle is judged by detecting the Euclidean distance between the predetermined trajectory of the unmanned vehicle and the predicted trajectory of the manually driven vehicle, and the collision condition is judged as follows: in a prediction time window t∈T window , if the Euclidean distance between the predetermined trajectory point of the unmanned vehicle and the predicted trajectory point of the manually driven vehicle is less than a set safety distance threshold d safe , it is considered that the trajectory coincidence occurs, and thus the collision risk exists: In the formula, Colision(T i ego Colision(t) represents the collision risk of the unmanned vehicle and the manually driven vehicle at time t, and takes the value of 1, indicating that the unmanned vehicle and the manually driven vehicle have the collision risk, and takes the value of 0, indicating that the unmanned vehicle and the manually driven vehicle do not have the collision risk.​ T ego representing a predetermined trajectory of the autonomous vehicle; x ego (t), y ego (t) represent the lateral and longitudinal coordinates, respectively, of a point of the predetermined trajectory of the autonomous vehicle at time t. The overall collision risk estimation of an autonomous vehicle's planned trajectory and a human-driven vehicle's predicted trajectory requires a weighted sum of risk values over all trajectory points on the trajectory, within a time window T window = [t0, t f ] and the collision probability is represented as: where R colision (T i ) represents the collision probability of the planned trajectory of the autonomous vehicle and the predicted trajectory of the human-driven vehicle; ζ(t) represents the time weight, designed as an exponential decay function, expressed as: In the formula, λ represents an attenuation coefficient; The total collision probability of the planned trajectory of the unmanned vehicle and the set of predicted trajectories of the manually driven vehicle is obtained by comprehensively considering the confidence and the collision probability of each predicted trajectory, and is represented as: In the formula, K represents the total number of predicted trajectories of the manually driven vehicle i; When the total collision risk probability of the full trajectory set predicted by the autonomous vehicle and the human-driven vehicle exceeds a set threshold, i.e., R colision,total >R safe The occupancy prediction trajectory set in the future time t∈T window is mapped into the space-time resource field to form a space-time risk cost field.

9. The path coordination planning method for mixed platoon conflict zones according to claim 8, wherein, The construction of the spatiotemporal risk cost field is as follows: quantifying the risk value of each space-time point in the space-time resource field, for each time t, each spatial position (x, y), define its basic risk value is represented as: The spatiotemporal risk cost field is obtained by traversing the involved spatiotemporal points, and reflects the collision risk level of different spatial positions at the current time by comprehensively considering the occupancy probability of each trajectory point and the distribution characteristics thereof in the spatiotemporal resource field.

10. The path coordination planning method of mixed-vehicle conflict zones according to claim 9, wherein, The expanded state node of the spatiotemporal hybrid A-star algorithm is represented as s(x, y, θ, t), wherein: x and y respectively represent the lateral coordinate and the longitudinal coordinate of the unmanned vehicle; θ is the heading angle of the unmanned vehicle; and t is the time; For each exploration, a position update satisfying the kinematic constraint is generated as follows: Δd = v n+1 • ΔT; θ 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 a time resolution; Δd represents a distance variation of the unmanned vehicle; v n+1 represents a speed of the unmanned vehicle at the n+1 node; Δθ represents a steering angle variation of the unmanned vehicle; δ j represents a front wheel steering angle of the unmanned vehicle; L represents an axle distance of the unmanned vehicle; θ n+1 , θ n respectively represent a heading angle of the unmanned vehicle at the n+1 node and the n node; x n+1 , y n+1 respectively represent a lateral coordinate of the unmanned vehicle at the n+1 node and the n node; y n+1 , y n respectively represent a longitudinal coordinate of the unmanned vehicle at the n+1 node and the n node; R r represents a turning radius of the unmanned vehicle; wherein: In order to ensure the rationality of the control of the unmanned vehicle, the constraints of acceleration, steering angle and speed are set, and are represented as: a min ≤a i ≤a max ; - δ max ≤ δ j ≤ δ max ; 0 < v n+1 ≤ v max； In the spatiotemporal hybrid A-star algorithm, the risk cost c(n) of the expansion step length is introduced to evaluate the cumulative collision risk of the future trajectory of the current node to the end point in the spatiotemporal resource, and the future trajectory is assumed to be a uniform speed Cartesian path from the current point to the end point. The spatiotemporal heuristic cost of the current node is: c(n) =∑ζ(t) Collision(t i ,T ego ,t) Therefore, the total cost function f(n) of the spatiotemporal hybrid A-star based on the dynamic risk field is represented as: f(n) = h(n) + g(n) + c(n); In the formula, h(n) represents the expected future cost of the current node to the target node; g(n) represents the cumulative historical cost from the starting node to the current node; and c(n) represents the spatiotemporal heuristic cost of the current node, wherein: h(n) = ||s n -s G ||2 = |x n -x G |+|y n -y G |+|t n -t G |; wherein s n , s G represent the space-time coordinates of the current node and the target node, respectively, s n = (x n , y n , t n ), s G = (x G , y G , t G ); g(n) = g(n - 1) + Ad + w v • |v n - v n-1 | + w ref • |v n - v ref | + w δ • |θ 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 the speed are smoothed to obtain a more comfortable trajectory, wherein: Path optimization objective function F p is represented as: minF p = ω s · f s (X) + ω r · f r (X) + ω l · f l (X); In the formula, f s (X) represents the path smoothness cost, which is used to reduce sharp turns or irregular changes in the trajectory, so as 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, so as to maintain the reasonableness 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 during driving are smooth, and frequent acceleration or deceleration is avoided; ω s , ω r , ω l respectively represent the weight coefficients of f s (X), f r (X), f l (X). Speed optimization objective function F v is represented as: minF v = ω v · f v (S) + ω a · f a (S) + ω jerk · f jerk (S); where f v (S) represents a velocity deviation cost, f v (S) =∑(v k -v ref ) 2 ; f a (S) represents an acceleration change cost, f jerk (S) represents a jerk change cost, f jerk (S) =∑(a k+1 -a k ) 2 .

Images