A system and method for assessing the motion risk of autonomous vehicles based on multi-modal trajectory prediction

Abstract

This invention discloses a motion risk assessment system and method for autonomous vehicles based on multi-mode trajectory prediction, comprising four sensors, a time synchronization module, a verification module, a local anomaly voting and fault detection counter, a trajectory correction and feature enhancement module, and an LSTM network. The four sensors include an INS (Inertial Navigation System) combined inertial navigation sensor, an inertial navigation IMU (Inertial Measurement Unit) sensor, a drive-by-wire chassis sensor, and a visual recognition sensor. The INS combined inertial navigation sensor includes GPS and an IMU. The verification module includes a χ² (X-ray) sensor. 2 The system includes checks for consistency in motion patterns and residual consistency; the LSTM network comprises an LSTM encoder, a classification decoder, and an LSTM decoder. This invention proposes a complete "detection-evaluation-decision" closed-loop solution. By constructing a multi-mode prediction network incorporating fault characteristics, not only is fault identification achieved, but the impact of faults on driving safety can also be accurately assessed, providing comprehensive safety decision-making support for autonomous driving systems.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to an autonomous vehicle motion risk assessment system and method based on multi-mode trajectory prediction, which is used to improve the safety and reliability of autonomous vehicles in the event of satellite navigation failure. Background Technology

[0002] In autonomous driving systems, the combined positioning of an integrated navigation system (INS) and an inertial navigation system (IMU) is a key technology for achieving high-precision vehicle position estimation. However, in complex environments such as urban canyons, tunnels, and overpasses, INS signals are susceptible to multipath effects, obstruction, or hardware failures, leading to anomalies in positioning data such as spike noise, constant offset, progressive drift, or data loss. If these faults are not detected and corrected in time, the autonomous driving system will make decisions based on incorrect positioning information, potentially causing serious safety accidents. For example, existing research has shown that some autonomous driving accidents are directly related to INS signal failure, especially in environments where satellite signals are obstructed, where vehicles may deviate from their intended path due to positioning drift, or even collide.

[0003] Currently, INS fault detection methods mainly rely on single-sensor analysis or multi-sensor consistency checks. Single-sensor methods typically set fixed thresholds based on signal quality metrics (such as signal-to-noise ratio and positioning variance), but these methods struggle to distinguish between multipath interference and hardware faults, and have low sensitivity to progressive drift. Multi-sensor consistency checks compare redundant data from IMU, vision, or wheel speed sensors with the INS output, identifying anomalies through residual analysis or chi-square tests. However, these methods usually only focus on instantaneous data differences, ignoring the temporal characteristics of vehicle motion patterns, thus easily leading to false detections in complex dynamic scenarios (such as sharp turns or acceleration / deceleration).

[0004] In risk assessment following a malfunction, traditional methods often rely on physical models or rule bases to predict the vehicle's future trajectory, but they struggle to effectively model the uncertainties in driving behavior under malfunction conditions. For example, when an INS (Inertial Navigation System) fails, the autonomous driving system may make compensatory steering or acceleration maneuvers due to accumulated lateral control errors, and deterministic models cannot quantify the risk probability of such actions. Furthermore, existing methods typically lack the ability to model multimodal motion patterns, leading to overly conservative or optimistic risk assessment results that fail to provide a reliable basis for safety decisions.

[0005] To address the aforementioned problems, this invention proposes an INS fault detection and risk assessment method that integrates multi-sensor data, interactive multi-model (IMM) algorithm, long short-term memory network (LSTM) trajectory prediction, and Monte Carlo sampling. Summary of the Invention

[0006] The purpose of this invention is to address the technical deficiencies in the existing technology by providing an autonomous vehicle motion risk assessment system based on multi-mode trajectory prediction.

[0007] Another aspect of the present invention is to provide a method for assessing the motion risk of autonomous vehicles based on multi-mode trajectory prediction.

[0008] The technical solution adopted to achieve the purpose of this invention is:

[0009] An autonomous vehicle motion risk assessment system based on multi-modal trajectory prediction includes four sensors, a time synchronization module, a verification module, a local anomaly voting and fault detection counter, a trajectory correction and feature enhancement module, and an LSTM network. The four sensors include an INS (Inertial Navigation System) combined inertial navigation sensor, an inertial navigation IMU (Inertial Measurement Unit) sensor, a drive-by-wire chassis sensor, and a visual recognition sensor. The INS combined inertial navigation sensor includes GPS and an IMU. The verification module includes a χ² (X-ray) sensor. 2 The tests include motion pattern consistency and residual consistency; the LSTM network includes an LSTM encoder, a classification decoder, and an LSTM decoder.

[0010] The time synchronization module receives asynchronous vehicle position information from four sensors, outputs time-synchronized vehicle position information from the four sensors, and inputs it into the verification module. The verification module's χ² value... 2 The test standardizes the residuals and calculates χ². 2 The statistical measure is compared with a preset threshold to determine global consistency. If a global anomaly is detected, a residual consistency test and a motion pattern consistency test are triggered. The residual consistency test calculates the local anomaly degree of each sensor. Motion pattern consistency test calculates the local anomaly degree of each sensor. Local anomaly degree voting and fault detection counter receive data from χ 2 Local anomaly time and local anomaly degree of inspection and The system outputs the INS diagnostic results and INS failure time, which are then input into the trajectory correction and feature enhancement module. Before the failure time, the trajectory correction and feature enhancement module uses the vehicle position information output by the INS sensor as the vehicle state vector. After the failure time, since the INS sensor has been determined to be faulty, it uses the average position estimate from the other three sensors. As the input state vector, the fault flag Er is added to the input state vector to form the corrected vehicle state vector u after the fault time. t The correction vector u of the vehicle state after the fault time. tThe fault feature enhancement trajectory sequence U, composed of the INS fault detection results, is input into the LSTM network. The LSTM network outputs a multimodal conditional probability distribution P(Y|X). Using the multimodal conditional probability distribution P(Y|X) as input, N sampling trajectories are generated using the Monte Carlo sampling method, denoted as N. total After kinematic constraint screening of all sampled trajectories, N trajectories that meet the kinematic constraints and exceed the safety threshold are counted. out_of_range Ratio as risk value

[0011] In the above technical solution, the vehicle position information from the four sensors includes the vehicle position information P from the INS integrated inertial navigation system. GPS =[x GPS ,y GPS ] T Vehicle location information P from IMU IMU =[x IMU ,y IMU ] T Vehicle location information P from drive-by-wire feedback Kin =[x Kin ,y Kin ] T Vehicle location information P from visual recognition Obs =[x Obs ,y Obs ] T The drive-by-wire chassis sensors include a vehicle speed sensor and a steering angle sensor, which provide feedback on the vehicle's speed and steering angle. The positioning data P is calculated using a vehicle kinematic model combined with the vehicle speed and steering angle. Kin =[x Kin ,y Kin ] T The visual recognition sensor uses the distance to detected static objects to inversely solve for the vehicle's position information P. Obs =[x Obs ,y Obs ] T .

[0012] Another aspect of the present invention provides a method for assessing vehicle operational risk using the multi-modal trajectory prediction-based autonomous vehicle motion risk assessment system, comprising the following steps:

[0013] Step 1: Acquire vehicle position information from the vehicle's INS combined inertial navigation sensor, drive-by-wire chassis sensor, inertial navigation IMU sensor, and visual recognition sensor. The drive-by-wire chassis sensor calculates the vehicle position information using the vehicle kinematic model combined with vehicle speed and turning angle. The visual recognition sensor uses the distance of the detected static object to inversely solve the vehicle position information. The INS vehicle position information and IMU vehicle position information are directly output by the INS. The asynchronous vehicle position information of the four sensors is synchronized in time to obtain vehicle position information of the four sensors in a unified time-synchronized data format.

[0014] Step 2: Perform a χ² test on the vehicle position information from the four time-synchronized sensors obtained in Step 1. 2 Test and calculate χ² 2 Statistics are used, and a preset threshold is used for comparison to determine global consistency. If a global anomaly is found, the global anomaly time is output. Simultaneously, residual consistency and motion pattern consistency checks are performed. The residual consistency check calculates the Euclidean distance residuals between each pair of vehicle position information from the four sensors, constructing a residual matrix. The residuals are then standardized, and the local anomaly degree of each sensor is calculated. The motion pattern consistency test constructs four vehicle motion models: uniform velocity (CV), uniform acceleration (CA), cooperative turning (CT), and uniform Jerk CJ. The interactive multi-model (IMM) algorithm is run in parallel on the vehicle position information from the four sensors. A Kalman filter is used to estimate the probability of each motion model, and the JS divergence between the model probability distribution matrices output by each sensor is calculated. A motion pattern estimation consistency matrix is ​​constructed, and the local anomaly degree of each sensor is calculated. It also outputs historical motion patterns, and if there is a local anomaly... and A value of 1 indicates that one sensor's test result is inconsistent with that of sensor i. If the local anomaly score is 1... and If the result is 2, it means that two sensors have different test results from sensor i, and so on.

[0015] Step 3: Local Anomaly Voting and Fault Detection Counter Receives Local Anomaly Time and Local Anomaly Degree Output from Step 2. and When the output results of the two consistency checks in step 2 are both greater than 2 for INS local anomaly and less than 1 for the local anomaly of the other sensors, it indicates that all four sensors point to INS failure and confirm that INS has failed. If the result of the voting counter indicates that INS is in a faulty state, the local anomaly time is recorded as the fault occurrence time and the fault result is output.

[0016] Step 4: Based on the INS fault occurrence time output in Step 3, before the fault time, the trajectory correction and feature enhancement module uses the vehicle position information output by the INS sensor as the first two dimensions of the state vector, and the third dimension is the fault flag bit Er, which is set to 0; after the fault time, since the INS sensor has already determined that there is a fault, the weighted estimate of the vehicle position information from the IMU, drive-by-wire chassis, and visual recognition is used. The first two dimensions of the state vector are used as data, and the third dimension is the fault flag Er, denoted as 1, to obtain the correction vector u of the vehicle state after the fault. t ;

[0017] Step 5: Construct a multi-mode trajectory prediction LSTM network, which includes an LSTM encoder, a classification decoder, and an LSTM decoder. This network will then convert the corrected vehicle state vector u obtained in Step 4 after the fault time into a multi-mode trajectory prediction LSTM network. t The fault feature-enhanced trajectory sequence U, composed of the INS fault detection results, is input to the LSTM encoder. The classification decoder statistically analyzes the historical motion patterns output in step 2 (motion pattern consistency check), calculates the probability mean within 0.5 seconds of the historical patterns, and concatenates it with the encoder's hidden state to enhance the features of the LSTM's hidden layer output. This enhanced trajectory is then input to the classification decoder, which outputs the probabilities P(m) of six yaw operations (three horizontal and two vertical). i |X);

[0018] The LSTM decoder uses a one-hot encoding method to combine the hidden layer with six yaw operation types, performs six conditional decodings to output the two-dimensional Gaussian distribution parameters Θ for each operation type, and calculates the yaw operation parameters for a given operation class m. i Given the historical trajectory X, the probability distribution P of the future position Y Θ (Y|X,m i );

[0019] According to Bayes' theorem, given the operation category m i Given the historical trajectory X, the probability distribution P of the future position Y Θ (Y|X,m i The probability P(m) of this yaw operation type i By multiplying |X) by the product, we can obtain the fused multimodal conditional probability distribution P(Y|X);

[0020] Step 6: Based on the multimodal conditional probability distribution P(Y|X) from Step 5, perform Monte Carlo sampling and quantization with kinematic constraints. First, sample motion modes according to the operation type probability distribution generated by the classification decoder. Then, generate trajectories corresponding to the two-dimensional Gaussian distribution parameters Θ generated by the LSTM decoder. Calculate each trajectory, and after kinematic constraint screening, statistically analyze the proportion of trajectories that meet the kinematic constraints and exceed the safety threshold as the risk value P.risk .

[0021] In the above technical solution, step 1 uses interpolation and time-shift interpolation to synchronize the asynchronous positioning data of the four positioning sources in time.

[0022] In the above technical solution, the χ mentioned in step 2 2 The formula for calculating the statistic is:

[0023]

[0024] In the formula, N pairs The number of sensor pairs, i.e., χ 2 The degrees of freedom of the distribution, N pairs =6, The square of the normalized residual vector of the k-th sensor pair is k, and the number of the k-th sensor pair is k.

[0025] In the above technical solution, the local anomaly degree obtained by the residual consistency test in step 2 is... The calculation formula is:

[0026]

[0027] In the formula, z i-j σ is the standardized residual vector between different vehicle position information calculated by different sensors. i-j The average location data from different sensors is calculated based on historical data, where i and j are the sensors;

[0028] The standardized residual vector z between different vehicle position information calculated by different sensors i-j The calculation formula is:

[0029]

[0030] In the formula, r i-j μ represents the residual between vehicle position information from different sensors. i-j and σ i-j These represent the mean and standard deviation of historical data; and the residual r between vehicle position information from different sensors. i-j The calculation formula is:

[0031]

[0032] In the formula, x i y i and x j y j Vehicle location information from different sensors.

[0033] In the above technical solution, the local anomaly degree obtained by the motion pattern consistency test in step 2 The calculation formula is:

[0034]

[0035] In the formula, SP i σ is a measure of the difference between the motion pattern prediction probability output by sensor i and the average motion pattern prediction probability. i|j Let be the total variance of the probability matrices between sensor i and the other sensors.

[0036] In the above technical solution, the calculation formula for the fault feature enhancement trajectory sequence U after the fault time by the trajectory correction and feature enhancement module in step 5 is as follows:

[0037] U = {u1, u} 2, u t ,…,u T}

[0038] In the formula, u t This is the correction vector for the vehicle state after the fault, where t is time, t = 1, 2, ..., T;

[0039] The correction vector u of the vehicle state after the fault. t The calculation formula is:

[0040]

[0041] In the formula, t f At the time of the fault, Er represents the vehicle fault flag; it is recorded as 0 if there is no fault, and 1 otherwise. k y k For the vehicle location information of sensor k, This is the average value of the vehicle position information estimated by the three sensors other than the INS sensor;

[0042] The average value of vehicle position information estimated from the three sensors other than the INS sensor. The calculation formula is:

[0043]

[0044] In the formula, i represents the three sensors other than the INS combined inertial navigation sensor. This refers to the vehicle position information obtained from the IMM algorithm of the i-th sensor, excluding the INS combined inertial navigation sensor.

[0045] In the above technical solution, the formula for calculating the multimodal conditional probability distribution P(Y|X) of the LSTM network output in step 5 is as follows:

[0046]

[0047] In the formula, P Θ (Y|X,m i ) is the output of the LSTM decoder in a given operation class m i Given the historical trajectory X, the probability distribution of the future position Y, where Θ is the parameter of the two-dimensional Gaussian distribution at each future time step, and P(m i |X) represents the probability that the vehicle will perform the i-th operation type, as output by the classification decoder.

[0048] The formula for calculating the two-dimensional Gaussian distribution parameter Θ at each future time step is:

[0049]

[0050] In the formula, t is the current time, (t+1) is the next time, and t f This represents the time interval between the occurrence of the fault and the current time, (t+t) f () represents the time elapsed after the fault occurrence interval, starting from the current moment.

[0051] In the above technical solution, the probability P(m) of the vehicle performing the i-th operation type is... i The formula for calculating |X) is:

[0052]

[0053] In the formula, and These are the probabilities of the p-th horizontal operation and the q-th vertical operation, respectively.

[0054] Compared with the prior art, the beneficial effects of the present invention are:

[0055] 1. This invention proposes an INS (In-Vehicle Assisted Driving) fault detection and risk assessment method that integrates multi-sensor data, the Interactive Multi-Model (IMM) algorithm, Long Short-Term Memory (LSTM) trajectory prediction, and Monte Carlo sampling. This method constructs a consistency verification framework for multi-source data through time synchronization and kinematic modeling, dynamically estimates the probability of vehicle motion modes using the IMM algorithm, and combines this with the LSTM network to predict the multimodal trajectory distribution under fault conditions. Finally, Monte Carlo sampling is used to statistically analyze the proportion of risky trajectories, outputting interpretable safety indicators. Experiments demonstrate that this method outperforms traditional solutions in terms of fault detection latency, recognition rate, and risk assessment accuracy, making it suitable for enhancing autonomous driving safety in complex environments.

[0056] 2. This invention addresses the INS (Instrument Sensor) fault detection problem in autonomous driving systems by innovatively proposing a dual verification mechanism that combines data layer residual consistency and model layer motion pattern consistency. This mechanism not only includes real-time multi-sensor data consistency verification but also innovatively introduces an analysis method based on historical motion patterns, significantly improving the fault identification rate while effectively reducing the possibility of false positives and false negatives.

[0057] 3. The core innovation of this invention lies in establishing a spatiotemporal joint fault identification framework. By analyzing the temporal evolution of motion pattern characteristics, the system can intelligently distinguish between real faults and transient interference, solving the problem of high misjudgment rate in complex scenarios using traditional methods. This method based on historical data analysis provides a new judgment dimension for fault detection.

[0058] 4. Regarding fault handling, this invention proposes a complete "detection-evaluation-decision" closed-loop solution. By constructing a multi-mode prediction network that includes fault characteristics, not only is fault identification achieved, but the impact of faults on driving safety can also be accurately assessed, providing comprehensive safety decision-making basis for autonomous driving systems. Attached Figure Description

[0059] Figure 1 This is a schematic diagram of the autonomous vehicle motion risk assessment system based on multi-mode trajectory prediction according to the present invention.

[0060] Figure 2 This is a graph illustrating the calculation of local anomalies based on the IMM algorithm of this invention.

[0061] Figure 3 This is a flowchart of the IMM filtering algorithm for a single sensor in this invention. Detailed Implementation

[0062] The present invention will be further described in detail below with reference to specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0063] Example 1

[0064] like Figure 1 As shown, an autonomous vehicle motion risk assessment system based on multi-mode trajectory prediction includes four sensors, a time synchronization module, a verification module, a local anomaly voting and fault detection counter, a trajectory correction and feature enhancement module, and an LSTM network. The four sensors include an INS combined inertial navigation sensor, an inertial navigation IMU sensor, a drive-by-wire chassis sensor, and a visual recognition sensor. The INS combined inertial navigation sensor includes GPS and an IMU. The verification module includes a χ²... 2The tests include motion pattern consistency and residual consistency; the LSTM network includes an LSTM encoder, a classification decoder, and an LSTM decoder.

[0065] The time synchronization module receives asynchronous vehicle position information from four sensors, outputs time-synchronized vehicle position information from the four sensors, and inputs it into the verification module. The verification module's χ² value... 2 The test standardizes the residuals and calculates χ². 2 The statistical measure is compared with a preset threshold to determine global consistency. If a global anomaly is detected, a residual consistency test and a motion pattern consistency test are triggered. The residual consistency test calculates the local anomaly degree of each sensor. Motion pattern consistency test calculates the local anomaly degree of each sensor. Local anomaly degree voting and fault detection counter receive data from χ 2 Local anomaly time and local anomaly degree of inspection and The system outputs the INS diagnostic results and INS failure time, which are then input into the trajectory correction and feature enhancement module. Before the failure time, the trajectory correction and feature enhancement module uses the vehicle position information output by the INS sensor as the vehicle state vector. After the failure time, since the INS sensor has been determined to be faulty, it uses the average position estimate from the other three sensors. As the input state vector, the fault flag Er is added to the input state vector to form the corrected vehicle state vector u after the fault time. t The correction vector u of the vehicle state after the fault time. t The fault feature enhancement trajectory sequence U, composed of the INS fault detection results, is input into the LSTM network. The LSTM network outputs a multimodal conditional probability distribution P(Y|X). Using the multimodal conditional probability distribution P(Y|X) as input, N sampling trajectories are generated using the Monte Carlo sampling method, denoted as N. total After kinematic constraint screening of all sampled trajectories, N trajectories that meet the kinematic constraints and exceed the safety threshold are counted. out_of_range The proportion as the risk value P risk .

[0066] Example 2

[0067] The method for assessing vehicle motion risk using the multi-modal trajectory prediction-based autonomous vehicle motion risk assessment system of Example 1 includes the following steps:

[0068] Step 1: Obtain vehicle position information from the vehicle's combined inertial navigation (INS) sensor. The vehicle position information is in the form of two-dimensional coordinates of the vehicle in UTM coordinates. Combined with the vehicle's drive-by-wire chassis sensor, inertial navigation (IMU) sensor, and visual recognition sensor, the vehicle position information of the four sensors is calculated. The drive-by-wire chassis sensor uses the vehicle kinematic model to calculate the vehicle position information by combining vehicle speed and turning angle. The visual recognition sensor uses the distance of the detected static object to inversely solve the vehicle position information. The INS vehicle position information and IMU vehicle position information are directly output by the INS. Time synchronization is performed on these asynchronous vehicle position information to obtain vehicle position information of the four sensors with a unified data format.

[0069] The vehicle position information from the four sensors includes the vehicle position information P from the INS integrated inertial navigation system. GPS =[x GPS ,y GPS ] T Vehicle location information P from IMU IMU =[x IMU ,y IMU ] T Vehicle location information P from drive-by-wire feedback Kin =[x Kin ,y Kin ] T Vehicle location information P from visual recognition obs =[x obs ,y obs ] T The drive-by-wire chassis sensors include a vehicle speed sensor and a steering angle sensor, which provide feedback on the vehicle's speed and steering angle. The positioning data P is calculated using a vehicle kinematic model combined with the vehicle speed and steering angle. Kin =[x Kin ,y Kin ] T The visual recognition sensor uses the distance to detected static objects to inversely solve for the vehicle's position information P. Obs =[x Obs ,y Obs ] T .

[0070] Through the vehicle system interface, the following data are collected in real time: positioning data output by the INS receiver, raw data output by the IMU (three-axis acceleration, three-axis angular velocity, timestamp), vehicle speed V and front wheel steering angle θ fed back by the vehicle chassis controller local area network (CAN bus), and position information (distance, angle) of static obstacles (such as streetlights and traffic signs) relative to the vehicle identified by the vision system.

[0071] Through actual measurements, the data transmission cycles of each sensor are as follows: INS (20ms), IMU (20ms), chassis feedback (20ms), and visual recognition (100ms). Since visual recognition has the lowest cycle, this invention sets the visual recognition sampling cycle of 100ms as the system's reference time axis. Interpolation calculations are required for the high-frequency sensor (INS / IMU / chassis) data to achieve time registration.

[0072] Frequency synchronization using interpolation: When historical high-frequency sensor data exists at the reference time, second-order Lagrange interpolation is used for interpolation. For example, at a reference time of 100ms, the INS observation value can be calculated from its measurements at 80ms, 100ms, and 120ms.

[0073]

[0074] Phase compensation using time-shift interpolation: For two sensors with the same frequency but a phase difference (e.g., INS and IMU are both 50Hz, but their data sampling times are inconsistent), this invention uses the following method to achieve time alignment:

[0075]

[0076] Where T is the sampling period and Δt is the measured timestamp offset difference.

[0077] The specific method for calculating the vehicle position information of the visual recognition sensor is as follows:

[0078] The bus motion model is simplified to a bicycle model, and it is assumed that the front and rear wheels are combined into a single wheel, the front wheel steers, and the front wheel drives; the vehicle speed (V) is the speed of the rear axle center along the longitudinal axis of the vehicle; dynamic factors such as tire slippage and suspension effects are ignored, and the vehicle body and suspension system are considered to be rigid systems.

[0079] Coordinate system definition: Global coordinate system: XY, the vehicle position is represented by the rear axle center coordinates (x, y); Vehicle coordinate system: x'-y', the origin is at the rear axle center, and the x' axis is along the vehicle's longitudinal axis; Heading angle: ψ, the angle between the vehicle's longitudinal axis and the global X-axis (counterclockwise is positive); State variables are x, y, and ψ (position and heading angle); Inputs are V (vehicle speed) and θ (front wheel steering angle).

[0080] When the vehicle rotates around the instantaneous center of rotation, with a turning radius R = L / tanθ, the rate of change of the heading angle ψ has the following differential equation:

[0081]

[0082] The differential equations for the lateral and longitudinal motions x and y are:

[0083]

[0084] Substituting the differential equations of lateral and longitudinal x and y motion into the differential equation of the rate of change of heading angle, we obtain the following differential equation, thus deriving the complete kinematic model:

[0085]

[0086] The vehicle speed V and steering angle θ are discretized data obtained from the drive-by-wire system. Therefore, the complete kinematic model is also discretized. Let the sampling time be Δt, and the Euler method is used for discretization, resulting in the following formula:

[0087]

[0088] Let the location information at the historical 3-second moment be x0 and y0.

[0089] Integrating the discretized and deformed complete kinematic model yields:

[0090]

[0091] By using visual recognition results to determine the vehicle's current position, and leveraging prior knowledge that the position of static objects (such as road signs and traffic lights, which are common static objects in urban environments) remains unchanged in the geodetic coordinate system, the vehicle's position change over the past 5 seconds can be calculated in real time.

[0092] The algorithm maintains a sliding window of length 5 seconds. Let the vehicle's UTM coordinates at historical time t0 be (x0, y0), and the heading angle be θ0. A static obstacle is selected as the solution reference, with its relative coordinates in the vehicle coordinate system being... By performing rotation and translation operations, it can be mapped to the UTM coordinate system, thus obtaining the position of the solution reference object in the UTM coordinate system:

[0093]

[0094] In the formula, R(θ0) is the rotation matrix between the vehicle coordinate system and the geodetic coordinate system. In the two-dimensional planar positioning problem, the vehicle coordinate system is set as V, with its origin coinciding with the vehicle's centroid. The x-axis points in the direction of the vehicle's movement, and the y-axis extends to the left perpendicular to the x-axis. The global coordinate system is the UTM coordinate system W.

[0095] When the vehicle's heading angle is θ, the rotation transformation between the two coordinate systems can be characterized by the rotation matrix R(θ):

[0096]

[0097] Since the static characteristics of the obstacle are known as prior knowledge, X obs With Y obsMaintaining this constant in subsequent derivations, we can calculate the vehicle's position within any effective detection time. At any time t, we still have:

[0098]

[0099] In the formula, x t Let be the vehicle's position in the UTM coordinate system at time t. θ t Let be the longitudinal distance, lateral distance, and yaw angle of the reference object in the visual recognition output at time t, respectively. Solving these equations simultaneously yields:

[0100]

[0101] Considering that static obstacles may temporarily disappear due to occlusion or exceeding the sensor's range within the 5-second time window of autonomous vehicle operation, the system ensures continuous calculation according to the following rules: A historical data pool is constructed, storing the type, ID, relative coordinates, and corresponding vehicle heading angle of all static obstacles within the past 5 seconds, forming a sliding time window.

[0102] Initialization: Select any static obstacle visible in the current frame as the solution reference and calculate its position in the UTM coordinate system.

[0103] Failure determination: If the reference object being solved is outside the sensor's field of view or there is no valid identification data within two consecutive time steps, switch to a new reference object being solved;

[0104] Reference Update: Select the static obstacle from the data pool that has the longest continuous effective detection time within a 5-second window as the new solution reference.

[0105] Iterative iteration: The solution position of the original reference obstacle at its last valid moment is used as the initial value for the next iteration. and The calculations are recursively performed using newly selected static obstacle data up to the current time.

[0106] The vehicle's current position information, calculated through visual recognition, is as follows:

[0107]

[0108] In the formula, θ k This represents the horizontal and vertical distances and the camera heading angle at the end of the effective time of the i-th solution reference.

[0109] If no reference is available at the initialization time or if there is a sub-interval within the 5-second sliding window that has no available reference information, then the fault detection algorithm that relies on this method is paused.

[0110] Step 2: Calculate the vehicle location information from the four sensors obtained in Step 1 using the χ² method. 2 Test and calculate χ² 2 The system uses statistical measures and preset thresholds for comparison to determine global consistency. If a global anomaly is found, the global anomaly time is output. Simultaneously, residual consistency and motion pattern consistency checks are performed. The residual consistency check calculates the pairwise Euclidean distance residuals between the vehicle position information of the four sensors, constructing a residual matrix. The residuals are then standardized, and the local anomaly degree C of each sensor is calculated. i The motion pattern consistency test constructs four vehicle motion models: uniform velocity (CV), uniform acceleration (CA), cooperative turning (CT), and uniform Jerk CJ. Four interactive multi-model (IMM) algorithms are run in parallel on the vehicle position information from the four sensors. Kalman filters are used to estimate the probability of each motion model, and the JS divergence between the model probability distribution matrices output by each sensor is calculated to construct a motion pattern estimation consistency matrix. The local anomaly degree of each sensor is then calculated. It also outputs historical motion patterns;

[0111] The main purpose of this step is to determine whether there is an overall inconsistency in the system by comparing the differences (residuals) in the output positions between different positioning sources, and to initially locate the sensors that may be faulty.

[0112] First, construct the residual matrix, and let the outputs of the four sensors at the same time be: INS vehicle position information: P GPS =[x GPS ,y GPS ] T IMU vehicle location information: P IMU =[x IMU ,y IMU ] T Drive-by-wire feedback of vehicle location information: P Kin =[x Kin ,y Kin ] T Visual recognition of vehicle location information: P Obs =[x Obs ,y Obs ] T .

[0113] The residual between INS and the kinematic solution position is denoted as r. GPS|Kin And so on for each other pair of sensors:

[0114]

[0115] Define the residual matrix R as the Euclidean distance between the positional differences of each sensor and the other sensors:

[0116]

[0117] Meanwhile, to ensure the rapid response of the fault diagnosis system, a hierarchical voting mechanism was established for residual consistency testing, introducing the χ² (chi) coefficient. 2 The detector is defined and the normalized residual vector z is defined:

[0118]

[0119] In the formula μ i-j and σ i-j The mean and standard deviation of historical data are used to characterize the distribution characteristics of residuals under fault-free conditions.

[0120] Construct χ 2 Statistical measures are used to comprehensively assess the consistency of multi-sensor residuals:

[0121]

[0122] Where N pairs The number of sensor pairs, i.e., χ 2 The degrees of freedom of the distribution (N in this invention) pairs =6).

[0123] At the same time, for X 2 The threshold γ of the statistic is based on χ² 2 The 1-α quantile of the distribution is determined.

[0124] If χ 2 If the value is greater than γ, a consistency anomaly flag is triggered. This is further combined with a hierarchical voting mechanism:

[0125] Preliminary judgment: χ 2 The detection result was marked as a global anomaly;

[0126] Secondary localization: Count the number of abnormal residual pairs involved by each sensor, and define the degree of local anomaly.

[0127]

[0128] In the formula, z i-j σ is the standardized residual vector between different vehicle position information calculated by different sensors. i-j The average location data from different sensors is calculated based on historical data, where i and j are the sensors;

[0129] like Figure 2 As shown, prior knowledge of vehicle motion (i.e., motion model) is used to further verify consistency by comparing the degree of conformity of different sensor data to these motion models, providing information complementary to residual verification.

[0130] This invention defines a set of motion models that can describe common vehicle motion states. The following four types are used:

[0131] (a) Uniform Motion Model (CV): The CV model assumes that the target's velocity in the horizontal and vertical directions remains constant over a future time period, maintaining a uniform linear motion. However, due to various disturbances in reality, the target will be subject to various external forces, causing fluctuations in the target's acceleration. The acceleration effects of the disturbance forces on the target in the horizontal and vertical directions can be regarded as independent Gaussian white noise, so the state of the target at time k can be modeled using the uniform motion model of the object.

[0132]

[0133] To describe the state of the target at time k using a uniform motion model, x k y k , The coordinates and accelerations of the target in the horizontal and vertical directions are respectively represented by the discrete form of the state-space equations for the uniform motion model. It can be represented as:

[0134]

[0135] In the formula, W k The zero-mean Gaussian white noise, F CV G CV Here, the transition matrix and noise driving matrix of the CV model are respectively:

[0136]

[0137] In the formula, T is the time interval.

[0138] (b) Uniformly Accelerated Motion Model (CA): The CA model assumes that the target's acceleration in the horizontal and vertical directions remains constant over a future time period, maintaining a state of uniformly accelerated linear motion. Similarly, due to various external forces affecting the target in the horizontal and vertical directions, the additional horizontal and vertical accelerations caused by these disturbances are treated as independent Gaussian white noise. This model describes the target's state under the uniformly accelerated motion model. Modeling:

[0139]

[0140] The state of the target under the uniformly accelerated motion model. Let x be the target's acceleration in the horizontal and vertical directions, respectively, and let y be the discrete form of the state-space equations for the uniformly accelerated motion process. It can be represented as:

[0141]

[0142] In the formula, W k The noise matrix is ​​composed of Gaussian white noise with a mean of 0, representing the disturbance to the object's acceleration, F. CA G CA Let be the transition matrix and the noise driving matrix of the uniform acceleration model, respectively:

[0143]

[0144] (c) Cooperative Turning Motion Model (CT): The CT model assumes that the target undergoes circular motion with constant angular velocity, direction, and magnitude over a future time interval. Unlike the linear models of uniform velocity and uniform acceleration, the CT model is nonlinear and requires linearization to be converted into discrete state-space equations. The description vector of the target's cooperative turning motion in the CT model is used to construct this model.

[0145]

[0146] In the formula, Let be the descriptive vector of the target's cooperative turning motion. When performing cooperative turning motion in a two-dimensional plane, its real-time position in global coordinates is:

[0147]

[0148] In the formula, x c y c The center of the target circular motion, Let ω be the initial angle of the target and ω be the angular velocity of the target.

[0149] The discrete state-space equations of the CT model's motion process It can be represented as

[0150]

[0151] In the formula, F TT This is the transition matrix for the cooperative turning motion model. G is the descriptive vector of the target's cooperative turning motion. CT W is the noise driving matrix of the cooperative turning motion model. k Gaussian white noise with zero mean.

[0152] The transition matrix F of the cooperative turning motion model CT Represented as:

[0153]

[0154] Noise driving matrix G of the cooperative turning motion model CT Represented as:

[0155]

[0156] (d) Uniform Jerk Model: The CJ model assumes that the rate of change of acceleration of the target is constant in the x and y components over a future period, maintaining a uniformly accelerated motion. However, due to external disturbances, the vehicle cannot perfectly guarantee a constant jerk value. Therefore, the additional jerk disturbance is modeled as Gaussian white noise, and the state matrix of the target's uniform jerk process is... Modeling:

[0157]

[0158] Discrete form of the state-space equations for uniform jerk motion It can be represented as:

[0159]

[0160] In the formula, F CJ G CJ Let be the transition matrix and the noise driving matrix of the uniform acceleration model, respectively. These can be represented in block matrix form as follows:

[0161]

[0162] Similarly, the noise matrix is ​​W. k It is Gaussian white noise with a mean of 0, representing the interference with the object's jerk value.

[0163] like Figure 2 , Figure 3 As shown, an IMM filter is run independently for the positioning data of each sensor, and each IMM filter contains Kalman filters corresponding to the above r=4 motion models.

[0164] The first step is input interaction: There are r models. Taking model j as an example, the algorithm has already obtained the target state estimate of model j at time k-1 at the start of the algorithm. covariance Model Probability And the state transition matrix P. The calculation process is as follows: the mixture probability from motion model i to motion model j.

[0165]

[0166]

[0167] In the formula, Let p be the predicted probability of model j, representing the probability that the target is in model j after the input interaction. ij Let i be the transition probability from model i to model j. Let be the probability of model i at time k-1. Let be the mixture state estimate of model j at time k-1. for:

[0168]

[0169] In the formula, Let i be the target state estimate of model i at time k-1. Let be the mixed probability from motion model i to motion model j.

[0170] Mixture covariance estimation of model j for:

[0171]

[0172] The Kalman filter is used in parallel filtering. First, the positioning data obtained from a certain sensor is input into four Kalman filters. The prediction models of the four Kalman filters are the four models mentioned in the previous section: CV, CA, CT, and CJ. Let model X... j For example, we have the following formula:

[0173] The state prediction equations are consistent with the discrete form of the state-space equations of the model:

[0174]

[0175] This is the prior prediction value obtained based on the optimal estimate from the previous step. Let J be the state transition matrix of motion model j at time k-1. Let G be the initial predicted state obtained at time k-1 based on the information at time k-2 under motion model j. j Let W be the process noise driving matrix of motion model j. k Let G be the process noise vector at time k, so G j W k Let be the state change of motion model j at time k caused by process noise.

[0176] Its error covariance prediction value

[0177]

[0178] In the formula, Let J be the state transition matrix of motion model j at time k-1. The initial state estimation covariance matrix of motion model j at time k-1 based on the information at time k-1 is given. Let j be the state transition matrix of motion model j at time k-1. The transpose matrix, G j Q is the process noise driving matrix of motion model j. j It is the process noise covariance matrix of motion model j. Let be the increment of the state covariance introduced by process noise in motion model j.

[0179] The Kalman gain is:

[0180]

[0181] In the formula, H j For the observation matrix, For the observation matrix h j The transpose of is given, and R is the observation noise covariance matrix. This represents the predicted value of the error covariance.

[0182] state Update and covariance The updated formula is:

[0183]

[0184] In the formula, This is the prior prediction value obtained based on the optimal estimate from the previous step (time k-1). For Kalman gain, Z k H is the observation value at time k. j Let I be the observation matrix and I be the identity matrix. The third step is model probability update: the probability of model j is updated using the likelihood function.

[0185]

[0186] In the formula, c is the normalization constant. The likelihood function of model j is

[0187]

[0188] In the formula, The distance between the observation value of the j-th model at time k and its prediction value at the previous time is called the residual. Let be the covariance matrix of the residuals.

[0189]

[0190] Finally, the probabilities of the four updated models are combined into a 4*1 probability matrix M.i At time k, the sensor i's estimated values ​​M for the four motion models i (k) is represented as:

[0191]

[0192] To measure the consistency of different sensors' judgments on the current motion pattern, the probability vector M output by any two sensors i and j is calculated. i With M j Jensen-Shannon (JS) divergence between:

[0193]

[0194] In the formula, A is the probability vector M output by any two sensors i and j. i With M j The average value of KL(M) i ||A) is the probability vector M of sensor i. i The Körberg-Klebler divergence between the probability distribution and the mean probability vector A is used to measure the degree of difference between two probability distributions, KL(M). j ||A) is the probability vector M of sensor j. j The Kuhlberg-Klebler divergence between the average probability vector A and the mean probability vector A.

[0195]

[0196] In the formula, M ix Let x be the model probability output by the x-th Kalman filter in the interactive multimode algorithm for sensor i.

[0197] Based on the above steps, the JS divergence of the four sensors is calculated in pairs. For ease of calculation, JS(M) is defined. i M i The equation ) = 0, meaning the JS divergence between the sensor and its own probability matrix is ​​0. The final motion pattern estimation consistency matrix P is obtained. consist :

[0198]

[0199] Therefore, when sensor i fails, the local anomaly degree of motion model consistency test is defined according to the 3σ principle.

[0200]

[0201] In the formula, SP i σ is a measure of the difference between the motion pattern prediction probability output by sensor i and the average motion pattern prediction probability. i|jLet be the total variance of the probability matrices between sensor i and the other sensors.

[0202] Step 3: Local Anomaly Voting and Fault Detection Counter Receives Local Anomaly Time and Local Anomaly Degree C Output from Step 2 i and When the output results of the two consistency checks in step 2 are both greater than 2 for INS local anomaly and less than 1 for the local anomaly of the other sensors, it indicates that all four sensors point to INS failure and confirm that INS has failed. If the result of the voting counter indicates that INS is in a faulty state, the local anomaly time is recorded as the fault occurrence time and the fault result is output.

[0203] This step integrates the results of the previous two steps to make a final INS fault diagnosis, determine the precise time of the INS fault, and compare the preliminary fault judgment results of residual consistency and motion pattern consistency. If both steps determine that the same sensor (e.g., INS) is faulty, then the sensor is finally confirmed to be faulty. The output diagnostic result is: "INS Fault". If only one step determines that a sensor is faulty, or if the two steps determine that the faulty sensors are different, or if multiple sensors are determined to be faulty, it can be handled according to a preset strategy, such as being marked as "Uncertain" or "Multiple positioning system faults". If both steps consider the systems to be consistent, the diagnostic result is: "All positioning systems are consistent".

[0204] To ensure the fault detection algorithm takes into account multi-sensor characteristics and balances response latency and false alarm rate, an anomaly degree fault detection counter is designed. This anomaly degree fault detection counter maintains a separate counter Vi for the INS, with an initial value of 0. The counter update logic mechanism is as follows:

[0205] If the INS combined inertial navigation sensor satisfies the following four inequalities simultaneously, then the counter Vi increases by 10 units; otherwise, Vi decreases by 1 unit:

[0206]

[0207] Determining the fault occurrence time requires using the results of the motion pattern consistency check (step 3) as a benchmark, as it reflects persistent inconsistencies at the motion model level. Record the timestamp at which the motion pattern local anomaly of the sensor ultimately diagnosed as faulty (e.g., GPS) first consistently exceeds a threshold. This time is the output fault occurrence time.

[0208] Step 4: Based on the INS fault occurrence time output in Step 3, before the fault time, the trajectory correction and feature enhancement module uses the vehicle position information output by the INS sensor as the first two dimensions of the state vector, and the third dimension is the fault flag bit Er, which is set to 0; after the fault time, since the INS sensor has already determined that there is a fault, the weighted estimate of the vehicle position information from the IMU, drive-by-wire chassis, and visual recognition is used. The first two dimensions of the state vector are used as data, and the third dimension is the fault flag Er, denoted as 1, to obtain the correction vector u of the vehicle state after the fault. t Specifically:

[0209] Based on the diagnostic results, potentially disturbed INS data are corrected, and a comprehensive state vector containing location, motion pattern, and fault state is constructed as input to the subsequent prediction model.

[0210] Before the fault time, the position output from the GPS system and the output of the IMM algorithm are combined to form a new vehicle state vector. After the fault time, the GPS data becomes invalid, so the mean position estimate from redundant sensors is used instead. Let the fault time be t. f Then the vehicle state vector u t Revised to:

[0211]

[0212] In the formula, x k y k This provides vehicle location information when the INS sensor is fault-free. Er is the vehicle fault flag bit; it is 0 when there is no fault and 1 otherwise. k is time, and t is the time. f This refers to the time of failure.

[0213] The average value of the vehicle position information estimated by the three sensors other than the INS sensor is calculated using the following formula:

[0214]

[0215] Step 5: Use a Long Short-Term Memory (LSTM) network to learn patterns from historical state sequences and predict various possible future driving behaviors (operation types) and their corresponding trajectory probability distributions. Specifically:

[0216] Construct a multi-mode trajectory prediction LSTM network, which includes an LSTM encoder, a classification decoder, and an LSTM decoder, and use the corrected vehicle state vector u obtained in step 4 after the fault time. tThe fault feature-enhanced trajectory sequence U, composed of the INS fault detection results, is input to the LSTM encoder. The classification decoder statistically analyzes the historical motion patterns output in step 2 (motion pattern consistency check), calculates the probability mean within 0.5 seconds of the historical patterns, and concatenates it with the encoder's hidden state to enhance the features of the LSTM's hidden layer output. This enhanced trajectory is then input to the classification decoder, which outputs the probabilities P(m) of six yaw operations (three horizontal and two vertical). i |X);

[0217] The LSTM decoder uses a one-hot encoding method to combine the hidden layer with six yaw operation types, performs six conditional decodings to output the two-dimensional Gaussian distribution parameters Θ for each operation type, and calculates the yaw operation parameters for a given operation class m. i Given the historical trajectory X, the probability distribution P of the future position Y Θ (Y|X,m i );

[0218] According to Bayes' theorem, given the operation category m i Given the historical trajectory X, the probability distribution P of the future position Y Θ (Y|X,m i The probability P(m) of this yaw operation type i By multiplying |X) by the product, we can obtain the fused multimodal conditional probability distribution P(Y|X);

[0219] In LSTM networks, the context vector output by the encoder is the hidden layer state henc, which is used to store short-term context information. Since the motion pattern and operation type have a more explicit relationship, feature enhancement design is performed at the classification decoding stage.

[0220] like Figure 1 As shown, the model designed in this invention is a single encoder-dual decoder structure. The LSTM encoder extracts temporal features, and the two decoders use the context vectors before and after feature enhancement, respectively. The input of the LSTM encoder consists of historical trajectory points (x... k y k The fault feature enhancement trajectory sequence U is composed of the fault status and the INS fault state.

[0221] U = {r1, u2, u} t ,…,u T}, where t=1,…T.

[0222]

[0223] Er represents the vehicle fault flag; it is 0 when there is no fault and 1 otherwise. This is for estimating the positions of the three sensors other than the INS sensor.

[0224] The above time-series data is processed by an LSTM encoder. The context vector output by the LSTM encoder is the hidden layer state henc, which is used to store short-term context information. For the trajectory parameter decoder of the LSTM decoder, its input is the hidden layer state output henc. After one-hot encoding and embedding, the LSTM decoder calculates the vehicle's two-dimensional Gaussian distribution parameter Θ at the future time T′. This invention assumes that the motion distribution under each operation type conforms to an independent bivariate Gaussian distribution, and outputs four two-dimensional Gaussian parameters for each operation type, ignoring the covariance. The formula for calculating the vehicle's two-dimensional Gaussian distribution parameter Θ at the future time T′ (defined as from the current time to the future time corresponding to the occurrence of the fault) is as follows:

[0225]

[0226]

[0227] In the formula, Θ represents the two-dimensional Gaussian distribution parameters of the vehicle at time T′ in the future. In this algorithm scenario, t represents the current time, and t+1 represents the next time. f t+t represents the time interval between the occurrence of the fault and the current time. f Let μ be the time elapsed after the fault occurrence interval, starting from the current moment. x μ y Let σ be the mean value in the x and y directions, respectively. x σ y These are the variances in the x and y directions, respectively. Based on the vehicle's two-dimensional Gaussian distribution parameters Θ under a given operation category m... i Given the historical trajectory X, calculate the probability distribution P of the future position Y. Θ (Y|X,m i ).

[0228] Since the relationship between motion patterns and operation types is more explicit, this invention incorporates feature enhancement design in the classification decoder, taking the average motion pattern probability from the past 0.5 seconds for each time step (0.1s).

[0229]

[0230] In the formula, Let be the probability of the k-th motion pattern, where k is the number of time steps and i is the i-th motion pattern. The calculation formula is:

[0231]

[0232] In the formula, The average value of the CV motion model probability in the IMM results of vehicle position information from the three sensors other than the INS sensor:

[0233]

[0234] The average values ​​of CA, CT, and CJ motion models in the IMM results and Calculate similarly.

[0235] Average probability of four motion patterns Form a four-dimensional vector This vector can represent the statistical characteristics of vehicle motion patterns within a historical 0.5s period. The hidden state h of the encoder output enc Concatenate into a new vector h cat :

[0236]

[0237] This vector is used as input to the classification decoder, combining three lateral operations and two longitudinal operations. The result is a fully connected layer with a softmax layer, outputting m for each of the six operation types. i The probability P(m) of the joint probability distribution i |X), its calculation formula is:

[0238]

[0239] Wherein, P(m) i |X) represents the probability that the vehicle will perform the i-th operation type. and Let be the probability of the p-th horizontal operation and the probability of the q-th vertical operation.

[0240] Based on the intuitive performance of simulation experiments, this invention designs six operation types for the loss of vehicle control, which consist of two longitudinal and three lateral movements: lateral (left yaw, right yaw, lane keeping) and longitudinal (acceleration, constant speed or deceleration).

[0241] For longitudinal movement, since the danger of acceleration of a vehicle under fault conditions is generally higher than that of deceleration and slowing movement, this invention defines an acceleration operation as the change in the average speed of a vehicle within a 0.5s range relative to the previous time step at a certain time step that exceeds 80% of its previous speed; otherwise, it is a constant speed or slowing movement.

[0242] For lateral movement, this invention defines the operation category for this time step as left yaw when the vehicle's position deviates more than 0.5 meters to the left from the reference line, and right yaw and so on. The operation category for other time steps is defined as lane keeping.

[0243] The goal of using an LSTM network is to predict the future position of a vehicle and output a multimodal conditional probability distribution for subsequent risk assessment. Therefore, the core of the model is to estimate the multimodal conditional probability distribution P(Y|X):

[0244]

[0245] In the formula, P Θ (Y|X,m i ) is in a given operation category m i Given the historical trajectory X, the probability distribution of the future position Y, P(m i |X) is an operation category belonging to m given a historical accumulation trajectory X. i The probability is given by i, where i represents one of the six operation categories, m1 represents accelerating left yaw, m2 represents accelerating right yaw, and so on.

[0246] Step 6: Based on the multimodal conditional probability distribution P(Y|X) from Step 5, perform Monte Carlo sampling and quantization with kinematic constraints. First, sample motion modes according to the operation type probability distribution generated by the classification decoder. Then, generate trajectories corresponding to the two-dimensional Gaussian distribution parameters Θ generated by the LSTM decoder. Calculate each trajectory, and after kinematic constraint screening, statistically analyze the proportion of trajectories that meet the kinematic constraints and exceed the safety threshold as the risk value P. risk .

[0247] The Monte Carlo sampling method is used for risk quantification. Monte Carlo sampling is a numerical computation method based on random sampling, used to approximate solutions to complex problems, such as integrals, expectations, and probability distributions. Its core idea is to generate a large number of random samples and use statistical principles to estimate the solution. The results improve with the increase in the number of samples, making it very suitable for solving problems that cannot be solved directly using system characteristic parameters.

[0248] Monte Carlo sampling is performed using the multimodal conditional probability distribution P(Y|X) output by the LSTM network as input. The Monte Carlo sampling process is as follows:

[0249] 1. Branch selection: First, randomly sample the six operation types and construct the cumulative distribution function of the operation types in a fixed order.

[0250]

[0251] Then, a random number generator with a uniform distribution between [0,1] is used to select the operation type used in this sampling.

[0252] 2. Trajectory Sampling and Stitching: Time is discretized for sampling, with an interval step size of 0.1s. Since vehicle positions conform to a two-dimensional normal distribution and are independent of each other, each time step has...

[0253]

[0254] Therefore, a Gaussian random number generator can be used directly to obtain and concatenate a complete trajectory from t∈[0,1].

[0255] 3. Repeat the above steps to obtain N sampling trajectories, denoted as N. total Because it involves assessing the risks of vehicle operation, this invention limits the running time of Monte Carlo sampling, terminating sampling when the number of samples reaches 1000 or when the sampling time exceeds 0.5 seconds.

[0256] 4. Finally, unreasonable trajectories are eliminated from all sampled trajectories based on kinematic constraints. The sliding window method is used to examine the maximum and minimum values ​​of the turning radius and acceleration for each trajectory, with a sliding window of 1 second in length.

[0257] 5. Risk Quantification Calculation

[0258] After sampling and generating multi-mode trajectories and selecting those that meet kinematic constraints, a safety assessment needs to be performed on each trajectory to quantify risk. This essentially involves a qualitative assessment and statistical analysis of the sampled data, divided into two steps: safety threshold determination and risk statistics. The safety threshold determination has two criteria:

[0259] Excessive lane crossing or yaw distance: According to Chinese road standards (JTGB01-2014), the width of a typical urban lane is 3.5 meters. A vehicle is considered to have crossed a lane when its furthest point is 1.75 meters away from the center of the lane. The yaw distance threshold is set to the same threshold as this regulation. The invention employs a 0.5-second sliding window mechanism to determine whether each trajectory is a risky trajectory. Specifically, the sliding window starts at t=0 and slides forward over time. When a significant deviation in the vehicle's direction of travel occurs, i.e., the deviation of the vehicle's trajectory from the target trajectory's heading angle exceeds a pre-set reasonable threshold, the trajectory within this sliding window is determined to be a risky trajectory. The number of all risky trajectories determined through this process is denoted as N. out_of_range This allows you to calculate the risk value of the vehicle's operation at the current moment.

[0260] The above description is only a preferred embodiment of the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A motion risk assessment system for autonomous vehicles based on multi-modal trajectory prediction, characterized in that, The application relates to a sensor fusion system for vehicle motion state recognition, which comprises four sensors, a time synchronization module, a verification module, a local anomaly degree voting and fault detection counter, a trajectory correction and feature enhancement module and an LSTM network; the four sensors comprise an INS combined inertial navigation sensor, an inertial navigation (IMU) sensor, a line control chassis sensor and a visual recognition sensor, the INS combined inertial navigation sensor comprises a GPS and an IMU; the verification module comprises verification, motion mode consistency verification and residual consistency verification; the LSTM network comprises an LSTM encoder, a classification decoder and an LSTM decoder; The time synchronization module receives asynchronous vehicle position information from four sensors, outputs time-synchronized vehicle position information from the four sensors, and inputs it into the verification module. The test standardizes the residuals and calculates... The statistical measure is compared with a preset threshold to determine global consistency. If a global anomaly is detected, a residual consistency test and a motion pattern consistency test are triggered. The residual consistency test calculates the local anomaly degree of each sensor. Motion pattern consistency test calculates the local anomaly degree of each sensor. Local anomaly degree voting and fault detection counter receive from Local anomaly time and local anomaly degree of inspection and The system outputs the INS diagnostic results and INS failure time, which are then input into the trajectory correction and feature enhancement module. Before the failure time, the trajectory correction and feature enhancement module uses the vehicle position information output by the INS sensor as the vehicle state vector. After the failure time, since the INS sensor has been determined to be faulty, it uses the average position estimate from the other three sensors. As an input state vector, a fault flag bit is added to the input state vector. Er Correction vector for vehicle state after failure time The correction vector for the vehicle state after the fault time. The fault feature enhancement trajectory sequence composed of INS fault detection results The input is fed into an LSTM network, and the LSTM network outputs a multimodal conditional probability distribution. Using the multimodal conditional probability distribution P(Y|X) as input, the Monte Carlo sampling method is employed to generate... N The sampling trajectory is denoted as After kinematic constraint screening of all sampled trajectories, trajectories that meet the kinematic constraints and exceed the safety threshold are counted. Ratio as risk value .

2. The autonomous vehicle motion risk assessment system based on multi-modal trajectory prediction as described in claim 1, characterized in that, The vehicle position information from the four sensors includes vehicle position information from INS integrated inertial navigation. Vehicle location information from IMU Vehicle location information fed back by drive-by Vehicle location information from visual recognition The drive-by-wire chassis sensors include a vehicle speed sensor and a steering angle sensor, which provide feedback on the vehicle's speed and steering angle. Positioning data is calculated using a vehicle kinematic model combined with the vehicle speed and steering angle. The visual recognition sensor uses the distance to detected static objects to inversely determine the vehicle's position information. .

3. A method for assessing vehicle operation risk using the multi-modal trajectory prediction-based autonomous vehicle motion risk assessment system as described in claim 1, characterized in that, Includes the following steps: Step 1: Acquire vehicle position information from the vehicle's INS combined inertial navigation sensor, drive-by-wire chassis sensor, inertial navigation IMU sensor, and visual recognition sensor. The drive-by-wire chassis sensor calculates the vehicle position information using the vehicle kinematic model combined with vehicle speed and turning angle. The visual recognition sensor uses the distance of the detected static object to inversely solve the vehicle position information. The INS vehicle position information and IMU vehicle position information are directly output by the INS. The asynchronous vehicle position information of the four sensors is synchronized in time to obtain vehicle position information of the four sensors in a unified time-synchronized data format. Step 2: Process the vehicle location information from the four time-synchronized sensors obtained in Step 1. Verification, calculation The statistics are compared with preset thresholds to make a global consistency judgment. If there is a global anomaly, the global anomaly time is output. At the same time, residual consistency test and motion pattern consistency test are performed. The residual consistency test calculates the Euclidean distance residuals between each pair of vehicle position information of the four sensors and constructs a residual matrix. The residuals are standardized, and the local anomaly degree of each sensor is calculated. The motion pattern consistency test constructs four vehicle motion models: uniform velocity (CV), uniform acceleration (CA), cooperative turning (CT), and uniform Jerk motion (CJ). The interactive multi-model (IMM) algorithm is run in parallel on the vehicle position information from the four sensors. A Kalman filter is used to estimate the probability of each motion model, and the JS divergence between the model probability distribution matrices output by each sensor is calculated. A motion pattern estimation consistency matrix is ​​constructed, and the local anomaly degree of each sensor is calculated. It also outputs historical motion patterns, and if the local anomaly is... and A value of 1 indicates that there is one sensor connected to the sensor. i The test results are inconsistent, if the local anomaly degree and A value of 2 indicates that there are two sensors connected to the sensor. i If the test results are inconsistent, then the same applies; Step 3: Local Anomaly Voting and Fault Detection Counter Receives Local Anomaly Time and Local Anomaly Degree Output from Step 2. and When the output results of the two consistency checks in step 2 are both greater than 2 for INS local anomaly and less than 1 for the local anomaly of the other sensors, it indicates that all four sensors point to INS failure and confirm that INS has failed. If the result of the voting counter indicates that INS is in a faulty state, the local anomaly time is recorded as the fault occurrence time and the fault result is output. Step 4: Based on the INS fault occurrence time output in Step 3, before the fault time, the trajectory correction and feature enhancement module uses the vehicle position information output by the INS sensor as the first two dimensions of the state vector, and the third dimension is the fault flag bit. Er , recorded as 0; after the fault time, since the INS sensor has already determined a fault, the weighted average of the vehicle position information from IMU, drive-by-wire chassis, and vision recognition is used. The first two dimensions of the state vector are data, and the third dimension is the fault flag bit. Er Let 1 be the value of the vehicle state correction vector after the fault occurs. ; Step 5: Construct a multi-mode trajectory prediction LSTM network, which includes an LSTM encoder, a classification decoder, and an LSTM decoder. This network will then convert the corrected vehicle state vector obtained in Step 4 after the fault time into a multi-mode trajectory prediction LSTM network. The fault feature enhancement trajectory sequence composed of INS fault detection results The input is fed into the LSTM encoder. The classification decoder statistically analyzes the historical motion patterns output in step 2 (motion pattern consistency check), calculates the average probability over the past 0.5 seconds, and concatenates it with the encoder's hidden state to enhance the features of the LSTM's hidden layer output. This is then fed into the classification decoder, which outputs the probabilities of six yaw operations (three horizontal and two vertical). ; The LSTM decoder uses a one-hot encoding method to combine the hidden layer with six yaw operation types, performing six conditional decoding steps to output the two-dimensional Gaussian distribution parameters for each operation type. And calculate in a given operation category and historical trajectory Future position probability distribution ; According to Bayes' theorem, given a class of operations and historical trajectory Future position probability distribution The probability of this yaw operation type By multiplying them, we can obtain the fused multimodal conditional probability distribution. ; Step 6, using the multimodal conditional probability distribution from step 5. Based on this, Monte Carlo sampling and quantization with kinematic constraints are performed. First, motion patterns are sampled according to the operation type probability distribution generated by the classification decoder, and then the corresponding two-dimensional Gaussian distribution parameters are generated by the LSTM decoder. Generate trajectories and calculate the value of each trajectory. After kinematic constraint screening, the proportion of trajectories that meet the kinematic constraints and exceed the safety threshold is used as the risk value. .

4. The method for assessing the motion risk of autonomous vehicles based on multi-modal trajectory prediction as described in claim 3, characterized in that, In step 1, interpolation and time-shift interpolation are used to synchronize the asynchronous positioning data of the four positioning sources in time.

5. The method for assessing the motion risk of autonomous vehicles based on multi-modal trajectory prediction as described in claim 3, characterized in that, The steps described in step 2 The formula for calculating the statistic is: In the formula, Let be the number of sensor pairs, i.e., the degrees of freedom of the χ² distribution. , For the first k The square of the standardized residual vector of each sensor pair For the first Number of sensor pairs.

6. The method for assessing the motion risk of autonomous vehicles based on multi-modal trajectory prediction as described in claim 3, characterized in that, The local anomaly degree obtained from the residual consistency test in step 2 The calculation formula is: In the formula, This refers to the standardized residual vector between different vehicle position information calculated by different sensors. To calculate the average of positioning data from different sensors based on historical data, and For sensors; Standardized residual vectors between different vehicle position information calculated by different sensors The calculation formula is: In the formula, The residuals between vehicle position information from different sensors. and These represent the mean and standard deviation of historical data; and the residuals between vehicle position information from different sensors. The calculation formula is: In the formula, , and , Vehicle location information from different sensors.

7. The method for assessing the motion risk of autonomous vehicles based on multi-modal trajectory prediction as described in claim 3, characterized in that, The local anomaly degree obtained from the motion pattern consistency test in step 2 The calculation formula is: In the formula, For sensors i A measure of the difference between the output motion pattern prediction probability and the average motion pattern prediction probability. For sensors i The total variance of the difference between the probability matrices of the other sensors and the probability matrices of the other sensors.

8. The method for assessing the motion risk of autonomous vehicles based on multi-modal trajectory prediction as described in claim 3, characterized in that, The trajectory correction and feature enhancement module described in step 5 enhances the fault feature trajectory sequence after the fault time. The calculation formula is: In the formula, This is the correction vector for the vehicle state after the fault occurs. For time, ; Correction vector for vehicle state after failure The calculation formula is: In the formula, At the time of failure, Er This represents the vehicle fault indicator; it is recorded as 0 if there is no fault, and as 1 otherwise. , For sensors k Vehicle location information, , This is the average value of the vehicle position information estimated by the three sensors other than the INS sensor; The average value of vehicle position information estimated from the three sensors other than the INS sensor. , The calculation formula is: In the formula, i The other three sensors besides the INS combined inertial navigation sensor. , In addition to the INS integrated inertial navigation sensor, the third i Vehicle location information from a single sensor using the IMM algorithm.

9. The method for assessing the motion risk of autonomous vehicles based on multi-modal trajectory prediction as described in claim 3, characterized in that, The LSTM network outputs a multimodal conditional probability distribution in step 5. The calculation formula is: In the formula, For the output of the LSTM decoder in a given operation category and historical trajectory Future position The probability distribution, Let the parameters be a two-dimensional Gaussian distribution for each future time step. Implement the first step for the vehicle output by the classification decoder. i The probability of each operation type; Two-dimensional Gaussian distribution parameters at each future time step The calculation formula is: In the formula, For the current moment, For the next moment, This represents the time elapsed since the fault occurred. This refers to the time elapsed after the fault occurrence interval, starting from the current moment.

10. The method for assessing the motion risk of autonomous vehicles based on multi-modal trajectory prediction as described in claim 9, characterized in that, The vehicle implements the first i The probability of each operation type The calculation formula is: In the formula, and No. p The probability of the first horizontal operation and the probability of the second horizontal operation q Each vertical operation probability.

Images