A risk matrix construction method based on DIP-IMM-KF algorithm

Abstract

This invention belongs to the field of traffic control systems and autonomous vehicle trajectory prediction, specifically a method for constructing a risk matrix based on the DIP-IMM-KF algorithm. The method comprises the following steps: Step 1: Constructing a risk field that considers multiple factors, including the autonomous vehicle's virtual mass, geometry, speed, acceleration, heading angle, and equivalent distance, to construct the vehicle's safety potential field influencing factors; Step 2: Probabilistically predicting vehicle trajectory intentions under a dynamic interactive perception interactive multi-model based on a priority order mechanism; and Step 3: Constructing a risk matrix based on the risk field and the dynamic interactive perception interactive multi-model. By constructing a discrete risk matrix, the method quantifies the risk matrix in complex and changing traffic environments and spatiotemporal characteristics, providing strong support for subsequent autonomous vehicle decision-making.

Description

Technical Field

[0001] The present invention belongs to the field of traffic control systems and autonomous vehicle trajectory prediction, and specifically relates to a risk matrix construction method driven by the DIP-IMM-KF (Interactive Multiple Model with Sequential Priority Mechanism) algorithm. Background Art

[0002] The rapid development of intelligent, connected vehicles has driven global transportation transformation and technological innovation, spurring in-depth research on autonomous vehicles. Autonomous vehicles are categorized into five levels based on their degree of automation: assisted driving (L1), partially automated driving (L2), conditionally automated driving (L3), highly automated driving (L4), and fully automated driving (L5). Currently, the majority of autonomous vehicles on the road are at Level 2, with some reaching Level 2++. These vehicles are capable of maneuvers such as following and lane changing, but due to the complexity of the road environment, drivers must remain vigilant and ready to take control at any time. Trajectory prediction plays a crucial role in autonomous driving scenarios, ensuring the safe operation of autonomous vehicles. By predicting the movement of other road users (including vehicles ahead, pedestrians, and cyclists), autonomous vehicles can anticipate potential hazards in complex scenarios (such as intersections), such as vehicles running red lights or pedestrians suddenly entering the road, enabling them to take braking or evasive action, effectively reducing the risk of collision. Regarding its impact on traffic efficiency, trajectory prediction provides insight into traffic flow dynamics ahead, enabling flexible speed adjustments and optimized route planning. For example, on roads with increased traffic but relatively smooth traffic, vehicles can accurately maintain following distance and speed based on the predicted constant speed of the vehicle ahead, thereby reducing unnecessary acceleration and deceleration. Furthermore, if a road is predicted to be congested or subject to an accident, resulting in speed restrictions, vehicles can switch to a more unobstructed route in advance, thus shortening travel time. In these ways, travel prediction helps improve the efficiency of the entire road network.

[0003] Although autonomous driving technology has made significant progress, the safety of autonomous vehicles is still widely questioned. To ensure the safety of autonomous vehicles operating on the road, artificial potential field methods have emerged. However, existing risk field models often do not fully consider the spatiotemporal characteristics of vehicles, resulting in insufficient rationality. In addition, trajectory prediction cannot make flexible decisions like the owner when faced with complex road environments, and the prediction results may lead to minor changes in the road environment and cause actual problems. Therefore, this study proposes a method that combines traffic scenarios with autonomous vehicle decision-making, combining risk fields with driver trajectory intention prediction to ensure vehicle safety and improve its ability to cope with complex traffic scenarios. In the process of autonomous driving technology development, optimizing and building a safer autonomous driving system will promote its widespread application and improve the safety of the entire transportation system.

[0004] Because the technology and methods used in autonomous vehicle trajectory prediction influence the vehicle's final decision-making and operational behavior, reasonable predictions are crucial for safer autonomous vehicle operation. However, in the trajectory prediction process, people often prioritize efficiency and real-time performance, employing methods based on vehicle dynamics, kinematics, and machine learning, using vehicle-specific factors or historical data to predict the vehicle's trajectory. This approach only enables autonomous vehicles to perform high-precision following or overtaking maneuvers, which is consistent with vehicle operation, but it cannot effectively handle complex traffic scenarios. Minor road changes, such as road damage or temporary repairs, cannot guarantee the safety of autonomous vehicles, defeating the core purpose of autonomous vehicle development. Furthermore, trajectory prediction accuracy is low, making it difficult for vehicles to maintain their predicted trajectory. This results in a lack of meaningful trajectory prediction and hinders effective information exchange between autonomous vehicles. Furthermore, the construction of risk fields only considers a subset of factors affecting vehicle safety, neglecting vehicle geometry, resulting in an inadequate and unscientific application of the subsequent risk matrix. In this social context, the construction of a risk matrix that is real-time and considers the safety and spatiotemporal characteristics of autonomous vehicles is of great significance. With the further development of autonomous vehicles, this new approach will further enhance their development and provide a sound guarantee for their safety.

[0005] Using a risk matrix to evaluate traffic systems fully considers the safety of autonomous vehicles and provides a feasible reference for subsequent decision-making based on safety, thereby promoting the development of autonomous driving technology. Combining risk fields with trajectory intention prediction methods can determine vehicle positions in real time, ensure safety, and provide an effective evaluation reference for trajectory planning. The patent name is "A method for establishing a driving risk field model" and the patent publication number is CN118709366A. This patent application considers the influence of the ownership driving style coefficient, object speed, and vehicle spacing, providing effective support for driving warning and path planning, but does not consider the impact of vehicle geometry on the safety potential field. The patent title is "A Radar Target Path Processing Method Using Interactive Multi-Models in the Internet of Vehicles," with patent publication number CN113538947A, and the patent title is "A Lane Information Perception Method Based on an Unmanned Intelligent Vehicle," with patent publication number CN113903010A. Both patents utilize the IMM interactive multi-model, but their use of historical data lacks rigor, resulting in low real-time accuracy and direct trajectory output via point traces. Initially, vehicle speed was output based on heading angle and GPS information, without considering data-based adjustments to the state transition equations of the EKF (Exergy Kalman Filter). Although the IMM model approach is widely used, predictions of measurement results have not yet been made, and in practical applications, limited real-time information is available. The patent title is "A Vehicle Cooperative Control Method Considering the Overall Risk Level of an Intersection," with patent publication number CN117994973A. This patent application uses a risk field-based approach to obtain risk values ​​for discrete grids and construct a risk assessment index to generate intersection trajectories. However, this method's grid risk assessment can still lead to risk conflicts between vehicles due to the uncertainty of surrounding vehicle motion and timing. Summary of the Invention

[0006] In view of the above problems, the purpose of the present invention is to provide a risk matrix construction method driven by the DIP-IMM-KF algorithm. By constructing a safety potential field with multiple influencing factors, it is applicable to various scenarios, considering the trajectory intention prediction under the IMM interactive perception interactive multi-model, improving the adaptability to the complexity of the road environment, and conducting in-depth research on the trajectory intention prediction of surrounding vehicles and the scope of influence, ensuring the safety of the vehicle's own operation, and finally constructing a discrete risk matrix to realize the quantification of the risk matrix under complex and changeable traffic environments and spatiotemporal characteristics, providing strong support for the subsequent decision-making of autonomous driving vehicles.

[0007] The present invention provides a risk matrix construction method based on the DIP-IMM-KF algorithm, which includes the following steps:

[0008] Step 1: Construct a risk field that considers multiple factors, including the virtual mass, geometry, speed, acceleration, heading angle, and equivalent distance of the autonomous vehicle, to construct the vehicle's safety potential field influencing factors;

[0009] Step 2: Probabilistic prediction of vehicle trajectory intention under dynamic interactive perception interactive multi-model based on priority order mechanism;

[0010] Step 3: Construct a risk matrix based on risk field and dynamic interactive perception interactive multi-model.

[0011] As a preferred embodiment of the present invention, step one also includes the following steps:

[0012] A1: Since the relevant factors of the target vehicle are defined as equivalent mass when the vehicle is in operation, the equivalent mass expression is obtained through analysis of actual vehicle data: M i =m i (1.566×10 -14 ·v 6.687 +0.3345);

[0013] Where: M i Expressed as the equivalent mass of target vehicle i, m i is the actual mass of the target vehicle i, and v is the vehicle speed;

[0014] A2: Construct the risk range based on the vehicle's geometric shape and use the ellipse formula to determine the collision distance. Connect any point B outside the ellipse with the vehicle's center of mass A, find the point C on the ellipse where they intersect, and calculate the distance between point C on the ellipse and point B outside the ellipse. Subtract the distance between the center of mass A and point C from the distance between the center of mass A and point B to obtain the collision distance d1:

[0015]

[0016] Where: θ is the heading angle of the vehicle; l is the length of the vehicle; w is the width of the vehicle; (x0, y0) is the center of mass of the vehicle; (x′, y′) is the position coordinate of any point outside the ellipse;

[0017] A3: Considering the vehicle length, width and distance parameters, let Among them, d′1 represents the collision distance related parameter based on geometric shape, that is, geometric pseudorange;

[0018] A4: Based on the different risks posed by vehicles at different locations approaching the target vehicle, a risk pseudo-distance is introduced to correct the actual distance of the target vehicle from surrounding vehicles when the risk is posed. Since the vehicle's center of mass is used as the coordinate point in the formula, the formula for the risk pseudo-distance d′2 is:

[0019]

[0020] Where: τ is the critical threshold of the safety distance; α is the unknown coefficient related to speed; e is the base of the natural logarithm;

[0021] A5: The relationship between the geometric pseudorange d′1 and the risk pseudorange d′2 is combined through a weighting factor to serve as the pseudorange for adjusting the risk factors of geometric size, speed, heading angle, and position in risk assessment. At the same time, the safety potential field is adapted to various road traffic environments by adjusting the two parameters ω1 and ω2. The resulting comprehensive pseudorange d′ is:

[0022]

[0023] Where: ω1 represents the weight factor of the geometric pseudo-distance d′1 related to the geometric shape, ω2 represents the weight factor of the risk pseudo-distance d′2 related to the risk influence around the vehicle;

[0024] A6: To prevent the vehicle from colliding, establish the vehicle risk field E based on the vehicle's circumscribed ellipse. v as follows:

[0025]

[0026] Where: λ, α, β, and δ are all unknown coefficients; |d′| represents the absolute value of the integrated pseudorange; a is the acceleration of the selected target vehicle; θ is the angle formed by any point around the target vehicle's center of mass (x0, y0), that is, the heading angle of the vehicle; if represents if; x * Represents the horizontal coordinate of a point on the road and the center of mass of the vehicle on a two-dimensional plane, y * It represents the vertical coordinate of any point on the road and the center of mass of the vehicle on the two-dimensional plane, and else represents otherwise.

[0027] As a preferred embodiment of the present invention, step 2 further includes the following steps:

[0028] B1: Obtain real-time status information of surrounding vehicles SV through the perception system, including longitudinal position x, longitudinal velocity v x , longitudinal acceleration a x , lateral position y, lateral velocity v y , lateral acceleration a y ;

[0029] B2: Sort the surrounding vehicles SV into a priority list, stipulating:

[0030] (1) When vehicles are in the same lane, the vehicle in front has priority over the vehicle behind;

[0031] (2) When vehicles are in adjacent lanes and have the same speed, the vehicle in front has priority over the vehicle behind. If the speeds are different, the priority is determined by the order in which they arrive at the target location.

[0032] (3) When a vehicle is in the merging lane, it has a higher priority than vehicles on the adjacent main road when in the mandatory lane change area;

[0033] Generate priority list C based on the collected surrounding vehicle SV information k , so that vehicles with high priority are predicted first;

[0034] B3: By describing the multimodal uncertainty of vehicle motion, introducing an interactive perception mechanism, and constructing a vehicle motion model set M SV ={m1,m2,m3,m4,m5,m6}, where m1,m2,m3,m4,m5,m6 represent the decision-making methods of different vehicles respectively;

[0035] B4: Calculate the dynamic transition probability of the vehicle motion model and fuse the interactive perception states based on the IMM-KF algorithm;

[0036]

[0037] π qp =P(m k =p|m k-1 =q);

[0038]

[0039] Where: c 1,p is the normalization coefficient of the vehicle motion model p, reflecting the weighting of the vehicle motion model transition probability and historical probability; is the probability of vehicle motion model q at the k-1th position; π qp is the transition probability value, i.e. the element of the Markov transition matrix; is the probability of the mixed model at k-1; is the posterior state estimate of the vehicle motion model q at k-1, and the superscript 1 represents C k The vehicle with the highest priority; represents the fusion state of the vehicle motion model p at k-1, P is the probability, m k Indicates the selection of vehicle motion model p at k; m k-1 Indicates that the vehicle motion model q is selected at k-1, q∈M represents that q is a model in the vehicle motion model set, that is,

[0040] Any one of {m1,m2,m3,m4,m5,m6}; and p∈M represents that p is any model in the vehicle motion model set except the vehicle motion model selected by q, and k represents the time step at the current moment;

[0041] B5: Using the Kalman filter prediction probability model, the prior state estimate of the vehicle motion model p is solved by adding the state transition equation to the product of the fusion state and the gain matrix.

[0042]

[0043] Where: represents the prior state estimate of the vehicle motion model p of vehicle 1 at position k; represents the block diagonal matrix set by the control gain, the longitudinal dynamics are controlled by F1, and the lateral dynamics are controlled by F2; represents the input matrix; is the posterior estimate;

[0044] B6: Generate predictions in the time domain from k+1 to k+N p The state sequence is used to calculate the loss function;

[0045]

[0046] Where: t = k + 1, k + 2, ... k + N p is the forecast horizon; represents the predicted state of the vehicle motion model p of vehicle 1 at time step t; Φ(t,k) represents the state transfer matrix at time k; Φ(t,i) represents the state transfer matrix at any time, that is, i is any time in the entire time range, when t>i, Φ(t,i)=F t-i , F t-i Indicates multiplying F by ti times to describe the state transition from i to t; it is used to accumulate the dynamic propagation effect; I = F, and by constructing a loss function, the cost of the vehicle motion model p is quantified. represents the external input or interference term of vehicle 1 using vehicle motion model p at i; N p Indicates the length of the prediction time domain, starting from the current time k, and predicting N p time steps;

[0047]

[0048] Where W x ,W y ,W ν ,W l All are weights; and Respectively represent the lateral acceleration and longitudinal acceleration of the vehicle motion model p at time t; and are the longitudinal velocity and lateral displacement at k respectively; and and represent the longitudinal velocity and lateral displacement of the reference, respectively; represents the loss of vehicle 1 at k using vehicle motion model p;

[0049] B7: Probability update and result output, through Normalize the model probability and use the maximum likelihood function to convert the loss into a likelihood value to reflect the goodness of fit of the normalized model, so as to calculate the weight in is the probability of vehicle motion model p of vehicle 1 at point k, and the largest probability value is determined as the optimal model, Represents the vehicle motion model selected by the vehicle with the highest priority at location k;

[0050] B8: Repeat B4-B7 to obtain the optimal model probability of all surrounding vehicles SV in the priority list, and perform trajectory prediction through quadratic programming.

[0051] As a preferred embodiment of the present invention, step three also includes the following steps:

[0052] C1: Discretize any traffic scene at a spatial scale and convert the risk field into a grid along the x and y directions. Different grid points represent the location of a vehicle, and the interval is set to 0.1 meters.

[0053] C2: By creating a four-dimensional risk matrix M risk (t′, x, y, veh), where t′ represents the time dimension, x is the x-axis coordinate in space, i.e., the longitudinal position of the vehicle, y is the y-axis coordinate in space, i.e., the lateral position of the vehicle, and veh represents the vehicle number. Fill in the risk value of each vehicle at time t′ and position (x, y) in the four-dimensional risk matrix;

[0054] C3: Based on dynamic interactive perception interactive multi-model, the probability distribution μ of all surrounding vehicles SV is obtained k The value of and the corresponding optimal model are used as output and become the input of the interactive module in the next cycle. The predicted trajectory of the optimal model is multiplied by the risk field constructed in step 1. The safety cost is estimated by using a set of parameters (x, y, t′). Therefore, the risk field at a certain moment in the future is expressed by the following formula:

[0055] Pot(t′)=E V +∑∑μ kE v (x(t′),y(t′),t′);

[0056] Among them, Pot(t′) represents the risk field value at a certain time t′, μ k represents the probability of all vehicle motion models selected by the kth vehicle;

[0057] C4: After determining the risk field, extract the dynamic risk matrix M for a specific vehicle k′ d_risk (t′,x,y), used to record the maximum risk value excluding the k′th vehicle, that is:

[0058]

[0059] Among them, v num Represents the total number of vehicles;

[0060] C5: Extract the static risk matrix M s_risk (x,y), records the minimum value of the static risk matrix in the entire time dimension of the dynamic risk matrix;

[0061]

[0062] C6: Based on the size of the risk value under the time coordinate, select the one with the smallest risk value as the basis for risk assessment on the risk matrix for the construction of the risk matrix.

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

[0064] 1. The present invention innovatively constructs a vehicle risk matrix. In view of the fact that traditional vehicle driving safety research usually focuses on the impact of a single factor or a simple combination on safety in model construction, such as only focusing on the road geometry or vehicle motion parameters, and then analyzing local risk points to adjust safety strategies. This application incorporates multi-dimensional factors such as the motion state, attributes, geometry and position risk of CAV vehicles into the CAV vehicle field model, comprehensively considering the differences in safety risks of vehicles of different types and speeds; by analyzing the external geometry of the vehicle, a reasonable circumscribed elliptical potential field is constructed to describe the risk of the vehicle, thus breaking through the convention of the traditional particle model. The importance of each role in different scenarios is flexibly adjusted through weight factors, thereby achieving a comprehensive, accurate and dynamic assessment and guarantee of vehicle driving safety. From the perspective of the synergistic effect of multiple factors, the inherent laws of vehicle safe driving are deeply explored, fully demonstrating the depth and breadth of the application of risk field theory in the transportation field, and realizing the refined processing of vehicle driving safety analysis.

[0065] 2. The present invention predicts vehicle intention by considering the uncertainty of surrounding vehicle motion. Traditional trajectory prediction methods mainly rely on historical trajectories and current states for short-term extrapolation, which is easily disturbed by environmental changes, resulting in reduced prediction stability and accuracy. This application combines Kalman filtering (KF) with dynamic interactive perception interactive multi-model (DIP-IMM) to form a DIP-IMM-KF algorithm for statistical prediction, further improving the robustness of the prediction. Kalman filter interaction can effectively process system dynamic characteristics and uncertainty information. The dynamic interactive perception interactive multi-model (DIP-IMM-KF) constructs a dynamic model that considers vehicle motion uncertainty, thereby improving the estimation performance of complex dynamic systems. By deeply analyzing vehicle intentions and eliminating the influence of environmental uncertainty, the future vehicle state is determined based on probability, thereby providing sufficient time and accurate guarantees for the statistical planning of autonomous driving vehicles. Through probabilistic statistical analysis, the system can accurately predict future motion states based on vehicle predictions, greatly enhancing the ability of autonomous driving vehicles to cope with complex dynamic scenarios and providing reasonable information for real-time trajectory planning. This intention-based prediction model provides important technical support for the practical application of autonomous driving technology.

[0066] 3. The present invention improves the risk matrix construction and application system. Traditional methods usually rely on simple linear prediction models of historical data or single risk assessment matrix construction methods. Previous risk matrix construction is usually directly based on vehicle historical data information or trajectory prediction results, without fully considering factors such as environmental uncertainty and vehicle intention changes, resulting in limited reliability and practicality of the matrix. This application combines lane intention prediction results to calculate the probability of risk values ​​generated by vehicles to grid points at future moments, constructs a risk matrix, and generates dynamic and static risk matrices for trajectory and path planning. The innovative risk matrix construction and application system effectively reduces the risk of environmental uncertainty, significantly improves vehicle safety and the feasibility of the risk matrix, and has unique advantages in expanding application scenarios and comprehensive consideration of multi-element traffic. It provides a new solution and technical framework for vehicle safe driving planning in complex traffic environments, and effectively promotes the innovation and progress of safety management technology in the transportation field. By discretizing traffic scenarios into grids, combining the trajectory intention prediction probability results, calculating the risk values ​​of grid points, and constructing dynamic and static risk matrices, trajectory prediction and risk matrix construction based on multi-technology integration can better adapt to complex and changing traffic environments, provide more reliable and detailed planning and protection for vehicle driving safety, and greatly improve the effectiveness and adaptability of the entire traffic operation safety management. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] By referring to the following description in conjunction with the accompanying drawings, and with a more complete understanding of the present invention, other objects and results of the present invention will become more clear and easy to understand. In the accompanying drawings:

[0068] Figure 1 This is a schematic diagram of the risk field of the present invention;

[0069] Figure 2 Schematic diagram of the risk matrix of the present invention. DETAILED DESCRIPTION

[0070] See Figure 1-2 The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0071] The embodiment of the present invention provides a risk matrix construction method based on the DIP-IMM-KF algorithm, comprising the following steps:

[0072] Step 1: Construct a risk field that considers multiple factors, including the virtual mass, geometry, speed, acceleration, heading angle, and equivalent distance of the autonomous vehicle, to construct the vehicle's safety potential field influencing factors, and reasonably calculate and apply the safety potential field influencing factors:

[0073] A1: Since the relevant factors of the target vehicle are defined as equivalent mass when the vehicle is in operation, the equivalent mass expression is obtained through analysis of actual vehicle data: M i =m i (1.566×10 -14 ·v 6.687 +0.3345);

[0074] Where: M i Expressed as the equivalent mass of target vehicle i, m i is the actual mass of the target vehicle i, and v is the vehicle speed;

[0075] A2: Construct the risk range based on the vehicle's geometric shape and use the ellipse formula to determine the collision distance. Connect any point B outside the ellipse with the vehicle's center of mass A, find the point C on the ellipse where they intersect, and calculate the distance between point C on the ellipse and point B outside the ellipse. Subtract the distance between the center of mass A and point C from the distance between the center of mass A and point B to obtain the collision distance d1:

[0076]

[0077] Where: θ is the heading angle of the vehicle; l is the length of the vehicle; w is the width of the vehicle; (x0, y0) is the center of mass of the vehicle; (x′, y′) is the position coordinate of any point outside the ellipse;

[0078] A3: Considering the vehicle length, width and distance parameters, let Among them, d′1 represents the collision distance related parameter based on geometric shape, that is, geometric pseudorange;

[0079] A4: Based on the different risks posed by vehicles at different locations approaching a target vehicle, the concept of risk pseudo-distance is introduced to correct the actual distance of the target vehicle when it faces risks from surrounding vehicles. This unifies the vehicle potential field function and prevents the subsequent use of different potential field functions for target vehicles in different areas. Since the vehicle center of mass is used as the coordinate point in the formula, the formula for risk pseudo-distance d′2 is:

[0080]

[0081] Where: τ is the critical threshold of the safety distance; α is the unknown coefficient related to speed; e is the base of the natural logarithm;

[0082] A5: The relationship between the geometric pseudorange d′1 and the risk pseudorange d′2 is combined through a weighting factor to serve as the pseudorange for adjusting the risk factors of geometric size, speed, heading angle, and position in risk assessment. At the same time, the safety potential field is adapted to various road traffic environments by adjusting the two parameters ω1 and ω2. The resulting comprehensive pseudorange d′ is:

[0083]

[0084] Where: ω1 represents the weight factor of the geometric pseudo-distance d′1 related to the geometric shape, ω2 represents the weight factor of the risk pseudo-distance d′2 related to the risk influence around the vehicle;

[0085] A6: Based on the above correction of mass and the definition of pseudo-range, in order to prevent the vehicle from colliding, the risk field E of the vehicle is established based on the circumscribed ellipse of the vehicle and the above influencing conditions. v as follows:

[0086]

[0087] Where: λ, α, β, and δ are all unknown coefficients; |d′| represents the absolute value of the integrated pseudorange; a is the acceleration of the selected target vehicle; θ is the angle formed by any point around the target vehicle's center of mass (x0, y0), that is, the heading angle of the vehicle; if represents if; x * Represents the horizontal coordinate of a point on the road and the center of mass of the vehicle on a two-dimensional plane, y * It represents the vertical coordinate of any point on the road and the center of mass of the vehicle on the two-dimensional plane, and else represents otherwise.

[0088] Step 2: Vehicle trajectory intention probability prediction based on the Dynamic Interactive Perception Interactive Multi-Model (DIP-IMM-KF) based on the priority order mechanism;

[0089] B1: Obtain real-time status information of surrounding vehicles SV through perception systems (such as cameras, radars), including longitudinal position x, longitudinal velocity v x , longitudinal acceleration a x , lateral position y, lateral velocity v y , lateral acceleration a y ;

[0090] B2: Sort the surrounding vehicles SV into a priority list, stipulating:

[0091] (1) When vehicles are in the same lane, the vehicle in front has priority over the vehicle behind;

[0092] (2) When vehicles are in adjacent lanes and have the same speed, the vehicle in front has priority over the vehicle behind. If the speeds are different, the priority is determined by the order in which they arrive at the target location.

[0093] (3) When a vehicle is in the merging lane, it has a higher priority than vehicles on the adjacent main road when in the mandatory lane change area;

[0094] Generate priority list C based on the collected surrounding vehicle SV information k , so that vehicles with high priority are predicted first;

[0095] B3: By describing the multimodal uncertainty of vehicle motion, introducing an interactive perception mechanism, and constructing a vehicle motion model set M SV ={m1,m2,m3,m4,m5,m6}, where m1,m2,m3,m4,m5,m6 represent the decision-making methods of different vehicles respectively; that is, m represents the decision-making of the vehicle, and the numbers 1-6 represent different decision-making methods;

[0096] B4: Calculate the dynamic transition probability of the vehicle motion model and fuse the interactive perception states based on the IMM-KF (Interacting Multiple Model-Kalman Filter) algorithm;

[0097]

[0098] π qp =P(m k =p|m k-1 =q);

[0099]

[0100] Where: c 1,p is the normalization coefficient of the vehicle motion model p, reflecting the weighting of the vehicle motion model transition probability and historical probability; is the probability of vehicle motion model q at the k-1th position; πqp is the transition probability value, i.e. the element of the Markov transition matrix; is the probability of the mixed model at k-1; is the posterior state estimate of the vehicle motion model q at k-1, and the superscript 1 represents C k The vehicle with the highest priority; represents the fusion state of the vehicle motion model p at k-1, P is the probability, m k Indicates the selection of vehicle motion model p at k; m k-1 Indicates that the vehicle motion model q is selected at k-1, that is, P(m k =p|m k-1 =q) refers to the probability P of vehicle k adopting vehicle motion model p at the current moment, given that vehicle k-1 adopted vehicle motion model q at the previous moment. q∈M represents that q is a model in the vehicle motion model set, that is, any one of {m1,m2,m3,m4,m5,m6}; and p∈M represents that p is any model in the vehicle motion model set except the vehicle motion model selected by q. k represents the time step at the current moment.

[0101] B5: Using the Kalman filter prediction probability model, the prior state estimate of the vehicle motion model p is solved by adding the state transition equation to the product of the fusion state and the gain matrix.

[0102]

[0103] Where: represents the prior state estimate of the vehicle motion model p of vehicle 1 at position k; represents the block diagonal matrix set by the control gain, the longitudinal dynamics are controlled by F1, and the lateral dynamics are controlled by F2; represents the input matrix; It is a posterior estimate; different from the traditional IMM-KF, it is assumed here that there is no observation noise, the posterior estimate is equal to the prior estimate, and the Kalman gain calculation is avoided.

[0104] B6: Generate predictions in the time domain from k+1 to k+N p The state sequence is used to calculate the loss function;

[0105]

[0106] Where: t = k + 1, k + 2, ... k + N p is the forecast horizon; represents the predicted state of the vehicle motion model p of vehicle 1 at time step t; Φ(t,k) represents the state transfer matrix at time k; Φ(t,i) represents the state transfer matrix at any time, that is, i is any time in the entire time range, when t>i, Φ(t,i)=F t-i , where F is also the basic state transfer matrix, which describes the evolution law of the system state in one time step, F t-i Indicates multiplying F by ti times to describe the state transition from i to t; it is used to accumulate the dynamic propagation effect and is the key step of DIP-IMM-KF; I = F, and by constructing a loss function, the cost of the vehicle motion model p is quantified. (F) has the same meaning as in B5. represents the external input or interference term of vehicle 1 using vehicle motion model p at i; N p Indicates the length of the prediction time domain, starting from the current time k, and predicting N p time steps;

[0107]

[0108] Where W x ,W y ,W v ,W l All are weights; and Respectively represent the lateral acceleration and longitudinal acceleration of the vehicle motion model p at time t; and are the longitudinal velocity and lateral displacement at k respectively; and and represent the longitudinal velocity and lateral displacement of the reference, respectively; represents the loss of vehicle 1 at k using vehicle motion model p;

[0109] B7: Probability update and result output, through Normalize the model probability and use the maximum likelihood function to convert the loss into a likelihood value to reflect the goodness of fit of the normalized model, so as to calculate the weight in is the probability of vehicle motion model p of vehicle 1 at point k, based on which the maximum probability value is determined as the optimal model, Represents the vehicle motion model selected by the vehicle with the highest priority at location k;

[0110] B8: Repeat B4-B7 to obtain the optimal model probability of all surrounding vehicles SV in the priority list, and perform trajectory prediction through quadratic programming.

[0111] Step 3: Construct a risk matrix based on risk field and dynamic interactive perception interactive multi-model,

[0112] C1: Discretize any traffic scenario at a spatial scale, converting the risk field into a grid along the x and y directions. Different grid points represent the location of a vehicle to better describe their positional relationships and road geometry. The interval is set to 0.1 meters to accurately capture changes in risk values.

[0113] C2: By creating a four-dimensional risk matrix M risk (t′, x, y, veh), where t′ represents the time dimension, x is the x-axis coordinate in space, i.e., the longitudinal position of the vehicle, y is the y-axis coordinate in space, i.e., the lateral position of the vehicle, and veh represents the vehicle number. Fill in the risk value of each vehicle at time t′ and position (x, y) in the four-dimensional risk matrix;

[0114] C3: Based on the dynamic interactive perception interactive multi-model (DIP-IMM-KF), the probability distribution μ of all surrounding vehicles SV is obtained k The value of and the corresponding optimal model are used as output and become the input of the interactive module in the next cycle. The predicted trajectory of the optimal model is multiplied by the risk field constructed in step 1. By using a set of parameters (x, y, t′) to estimate the safety cost, computing time is saved and the feasibility of the trajectory is improved. Therefore, the risk field at a certain moment in the future is expressed by the following formula:

[0115] Pot(t′)=E V +∑∑μ k E v (x(t′),y(t′),t′);

[0116] Among them, Pot(t′) represents the risk field value at a certain time t′, μ k represents the probability of all vehicle motion models selected by the kth vehicle;

[0117] C4: After determining the risk field, extract the dynamic risk matrix M for a specific vehicle k′ d_risk (t′,x,y), used to record the maximum risk value excluding the k′th vehicle, that is:

[0118]

[0119] Among them, v num Represents the total number of vehicles;

[0120] C5: Extract the static risk matrix M s_risk (x,y), records the minimum value of the static risk matrix in the entire time dimension of the dynamic risk matrix;

[0121]

[0122] C6: Based on the size of the risk value under the time coordinate, select the one with the smallest risk value as the basis for risk assessment on the risk matrix for the construction of the risk matrix.

[0123] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A risk matrix construction method based on the DIP-IMM-KF algorithm, characterized in that: The following steps are involved: Step 1: Construct a risk field that considers multiple factors, including the virtual mass, geometry, speed, acceleration, heading angle, and equivalent distance of the autonomous vehicle, to construct the vehicle's safety potential field influencing factors; A1: Since the relevant factors of the target vehicle are defined as equivalent mass when the vehicle is in operation, the equivalent mass expression is obtained through analysis of actual vehicle data: M i =m i (1.566×10 -14 ·v 6.687 +0.3345); Where: M i Expressed as the equivalent mass of target vehicle i, m i is the actual mass of the target vehicle i, and v is the vehicle speed; A2: Construct the risk range based on the vehicle's geometric shape and use the ellipse formula to determine the collision distance. Connect any point B outside the ellipse with the vehicle's center of mass A, find the point C on the ellipse where they intersect, and calculate the distance between point C on the ellipse and point B outside the ellipse. Subtract the distance between the center of mass A and point C from the distance between the center of mass A and point B to obtain the collision distance d1: Where: θ is the heading angle of the vehicle; l is the length of the vehicle; w is the width of the vehicle; (x0, y0) is the center of mass of the vehicle; (x′, y′) is the position coordinate of any point outside the ellipse; A3: Considering the vehicle length, width and distance parameters, let Among them, d′1 represents the collision distance related parameter based on geometric shape, that is, geometric pseudorange; A4: Based on the different risks posed by vehicles at different locations approaching the target vehicle, a risk pseudo-distance is introduced to correct the actual distance of the target vehicle from surrounding vehicles when the risk is posed. Since the vehicle's center of mass is used as the coordinate point in the formula, the formula for the risk pseudo-distance d′2 is: Where: τ is the critical threshold of the safety distance; α is the unknown coefficient related to speed; e is the base of the natural logarithm; A5: The relationship between the geometric pseudorange d′1 and the risk pseudorange d′2 is combined through a weighting factor to serve as the pseudorange for adjusting the risk factors of geometric size, speed, heading angle, and position in risk assessment. At the same time, the safety potential field is adapted to various road traffic environments by adjusting the two parameters ω1 and ω2. The resulting comprehensive pseudorange d′ is: Where: ω1 represents the weight factor of the geometric pseudo-distance d′1 related to the geometric shape, ω2 represents the weight factor of the risk pseudo-distance d′2 related to the risk influence around the vehicle; A6: To prevent the vehicle from colliding, establish the vehicle risk field E based on the vehicle's circumscribed ellipse. v as follows: Where: λ, α, β, and δ are all unknown coefficients; |d′| represents the absolute value of the integrated pseudorange; a is the acceleration of the selected target vehicle; θ is the angle formed by any point around the target vehicle's center of mass (x0, y0), that is, the heading angle of the vehicle; if represents if; x * Represents the horizontal coordinate of a point on the road and the center of mass of the vehicle on a two-dimensional plane, y * It represents the vertical coordinate of any point on the road and the center of mass of the vehicle on the two-dimensional plane, and else represents otherwise; Step 2: Probabilistic prediction of vehicle trajectory intention under dynamic interactive perception interactive multi-model based on priority order mechanism; B1: Obtain real-time status information of surrounding vehicles SV through the perception system, including longitudinal position x, longitudinal velocity v x , longitudinal acceleration a x , lateral position y, lateral velocity v y , lateral acceleration a y ; B2: Sort the surrounding vehicles SV into a priority list, stipulating: (1) When vehicles are in the same lane, the vehicle in front has priority over the vehicle behind; (2) When vehicles are in adjacent lanes and have the same speed, the vehicle in front has priority over the vehicle behind. If the speeds are different, the priority is determined by the order in which they arrive at the target location. (3) When a vehicle is in the merging lane, it has a higher priority than vehicles on the adjacent main road when in the mandatory lane change area; Generate priority list C based on the collected surrounding vehicle SV information k , so that vehicles with high priority are predicted first; B3: By describing the multimodal uncertainty of vehicle motion, introducing an interactive perception mechanism, and constructing a vehicle motion model set M SV ={m1,m2,m3,m4,m5,m6}, where m1,m2,m3,m4,m5,m6 represent the decision-making methods of different vehicles respectively; B4: Calculate the dynamic transition probability of the vehicle motion model and fuse the interactive perception states based on the IMM-KF algorithm; π qp =P(m k =p|m k-1 (=q); Where: c 1,p is the normalization coefficient of the vehicle motion model p, reflecting the weighting of the vehicle motion model transition probability and historical probability; is the probability of vehicle motion model q at the k-1th position; π qp is the transition probability value, i.e. the element of the Markov transition matrix; is the probability of the mixed model at k-1; is the posterior state estimate of the vehicle motion model q at k-1, and the superscript 1 represents C k The vehicle with the highest priority; represents the fusion state of the vehicle motion model p at k-1, P is the probability, m k Indicates the selection of vehicle motion model p at k; m k-1 Indicates that the vehicle motion model q is selected at k-1, q∈M represents that q is a model in the vehicle motion model set, that is, Any one of {m1,m2,m3,m4,m5,m6}; and p∈M represents that p is any model in the vehicle motion model set except the vehicle motion model selected by q, and k represents the time step at the current moment; B5: Using the Kalman filter prediction probability model, the prior state estimate of the vehicle motion model p is solved by adding the state transition equation to the product of the fusion state and the gain matrix. Where: represents the prior state estimate of the vehicle motion model p of vehicle 1 at position k; represents the block diagonal matrix set by the control gain, the longitudinal dynamics are controlled by F1, and the lateral dynamics are controlled by F2; represents the input matrix; is the posterior estimate; B6: Generate predictions in the time domain from k+1 to k+N p The state sequence is used to calculate the loss function; Where: t = k + 1, k + 2, ... k + N p is the forecast horizon; represents the predicted state of the vehicle motion model p of vehicle 1 at time step t; Φ(t,k) represents the state transfer matrix at time k; Φ(t,i) represents the state transfer matrix at any time, that is, i is any time in the entire time range, when t>i, Φ(t,i)=F t-i , F t-i Indicates multiplying F by ti times to describe the state transition from i to t; it is used to accumulate the dynamic propagation effect; I = F, and by constructing a loss function, the cost of the vehicle motion model p is quantified. represents the external input or interference term of vehicle 1 using vehicle motion model p at i; N p Indicates the length of the prediction time domain, starting from the current time k, and predicting N p time steps; Where W x ,W y ,W v ,W l All are weights; and Respectively represent the lateral acceleration and longitudinal acceleration of the vehicle motion model p at time t; and are the longitudinal velocity and lateral displacement at k respectively; and and represent the longitudinal velocity and lateral displacement of the reference, respectively; represents the loss of vehicle 1 at k using vehicle motion model p; B7: Probability update and result output, through Normalize the model probability and use the maximum likelihood function to convert the loss into a likelihood value to reflect the goodness of fit of the normalized model, so as to calculate the weight in is the probability of vehicle motion model p of vehicle 1 at point k, and the largest probability value is determined as the optimal model, Represents the vehicle motion model selected by the vehicle with the highest priority at location k; B8: Repeat B4-B7 to obtain the optimal model probability of all surrounding vehicles SV in the priority list, and perform trajectory prediction through quadratic programming; Step 3: Construct a risk matrix based on risk field and dynamic interactive perception interactive multi-model; C1: Discretize any traffic scene at a spatial scale and convert the risk field into a grid along the x and y directions. Different grid points represent the location of a vehicle, and the interval is set to 0.1 meters. C2: By creating a four-dimensional risk matrix M risk (t′, x, y, veh), where t′ represents the time dimension, x is the x-axis coordinate in space, i.e., the longitudinal position of the vehicle, y is the y-axis coordinate in space, i.e., the lateral position of the vehicle, and veh represents the vehicle number. Fill in the risk value of each vehicle at time t′ and position (x, y) in the four-dimensional risk matrix; C3: Based on dynamic interactive perception interactive multi-model, the probability distribution μ of all surrounding vehicles SV is obtained k The value of and the corresponding optimal model are used as output and become the input of the interactive module in the next cycle. The predicted trajectory of the optimal model is multiplied by the risk field constructed in step 1. The safety cost is estimated by using a set of parameters (x, y, t′). Therefore, the risk field at a certain moment in the future is expressed by the following formula: Pot(t′)=E V +∑∑μ k E v (x(t′),y(t′),t′); Among them, Pot(t′) represents the risk field value at a certain time t′, μ k represents the probability of all vehicle motion models selected by the kth vehicle; C4: After determining the risk field, extract the dynamic risk matrix M for a specific vehicle k′ d_risk (t′,x,y), used to record the maximum risk value excluding the k′th vehicle, that is: Among them, v num Represents the total number of vehicles; C5: Extract the static risk matrix M s_risk (x,y), records the minimum value of the static risk matrix in the entire time dimension of the dynamic risk matrix; C6: Based on the size of the risk value under the time coordinate, select the one with the smallest risk value as the basis for risk assessment on the risk matrix for the construction of the risk matrix.

Images