Automatic driving vehicle motion risk assessment system and method based on multimode trajectory prediction

Abstract

The invention discloses an automatic driving vehicle motion risk assessment system and method based on multimode trajectory prediction. The system comprises four sensors, a time synchronization module, an inspection module, a local anomaly voting and fault detection counter, a trajectory correction and feature enhancement module and an LSTM network. The four sensors comprise an INS (inertial navigation system) integrated inertial navigation sensor, an inertial navigation IMU (inertial measurement unit) sensor, a drive-by-wire chassis sensor and a visual identification sensor; the INS integrated inertial navigation sensor comprises a GPS (global positioning system) and an IMU (inertial measurement unit); the checking module comprises x2 checking, motion mode consistency checking and residual error consistency checking; the LSTM network includes an LSTM encoder, a classification decoder, and an LSTM decoder. The invention provides a complete'detection-evaluation-decision 'closed-loop solution. By constructing the multi-mode prediction network containing the fault features, fault recognition is achieved, the influence degree of the fault on driving safety can be accurately evaluated, and a comprehensive safety decision basis is provided for an automatic driving system.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a system and method for assessing the motion risk of an autonomous driving vehicle based on multi-modal trajectory prediction, for improving the safety and reliability of the autonomous driving vehicle in the event of satellite navigation failure. Background Art

[0002] In autonomous driving systems, the combined positioning of an integrated navigation system (INS) and an inertial navigation unit (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, occlusions, or hardware failures, resulting in anomalies such as spike noise, constant offset, gradual drift, or data loss in positioning data. If these failures are not detected and corrected promptly, the autonomous driving system will make decisions based on erroneous positioning information, leading to serious safety accidents. For example, studies have shown that some autonomous driving accidents are directly related to INS signal failures. Especially in environments where satellite signals are obstructed, the vehicle may deviate from the planned path due to positioning drift, or even collide.

[0003] Currently, INS fault detection methods primarily rely on single-sensor analysis or multi-sensor consistency testing. Single-sensor methods typically set fixed thresholds based on signal quality metrics (such as signal-to-noise ratio and positioning variance). However, these methods struggle to distinguish between multipath interference and hardware failures and are less sensitive to progressive drift. Multi-sensor consistency testing methods compare redundant data from sensors such as IMUs, vision, or wheel speed sensors with INS outputs, identifying anomalies through residual analysis or chi-square tests. However, these methods typically focus only on instantaneous data differences and ignore the temporal characteristics of vehicle motion patterns. Consequently, they are prone to false detections in complex dynamic scenarios (such as sharp turns or acceleration and deceleration).

[0004] Traditional methods for post-failure risk assessment rely on physical models or rule bases to predict the vehicle's future trajectory. However, these methods struggle to effectively model the uncertainty inherent in driving behavior under fault conditions. For example, when an INS fails, the autonomous driving system may implement compensatory steering or acceleration maneuvers due to accumulated lateral control errors. However, deterministic models cannot quantify the risk probability of such actions. Furthermore, existing methods often lack the ability to model multimodal motion patterns, resulting in overly conservative or optimistic risk assessments that fail to provide a reliable basis for safety decision-making.

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

[0006] The purpose of the present invention is to provide an autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction to address the technical defects existing in the prior art.

[0007] Another aspect of the present invention is to provide a method for assessing the motion risk of an autonomous driving vehicle based on multimodal trajectory prediction.

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

[0009] A motion risk assessment system for an autonomous driving vehicle 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 combined inertial navigation sensor, an inertial navigation IMU sensor, a wire-controlled chassis sensor, and a visual recognition sensor; the INS combined inertial navigation sensor includes GPS and IMU; the verification module includes a χ 2 Test, motion pattern consistency test and residual consistency test; 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 the vehicle position information of the four sensors after time synchronization, and inputs it to the inspection module. 2 The test standardizes the residuals and calculates the χ 2 Statistics are compared with the preset threshold to make a global consistency judgment; if the global anomaly occurs, the residual consistency test and motion pattern consistency test are triggered, and the residual consistency test calculates the local anomaly degree of each sensor Movement pattern consistency test calculates the local anomaly of each sensor The local anomaly voting and fault detection counter receives the 2 Local abnormality time and local abnormality degree of inspection and Output INS diagnosis results and INS failure time, and input them into the trajectory correction and feature enhancement module. The trajectory correction and feature enhancement module uses the vehicle position information output by the INS sensor as the vehicle state vector before the failure time according to the INS failure time. After the failure time, since the INS sensor has been determined to be faulty, the position estimation mean of the remaining three sensors is used. As the input state vector, the input state vector adds the fault flag Er to form the correction vector u of the vehicle state after the fault moment. t ; The correction vector u of the vehicle state after the fault moment tThe fault feature enhanced trajectory sequence U composed of the fault detection results of INS is input into the LSTM network. The LSTM network outputs the multimodal conditional probability distribution P(Y|X). With the multimodal conditional probability distribution P(Y|X) as input, the Monte Carlo sampling method is used to generate N sampling trajectories, which are recorded as N total After all sampled trajectories are screened by kinematic constraints, the trajectories N that meet the kinematic constraints and exceed the safety threshold are counted. out_of_range Proportion as risk value

[0011] In the above technical solution, the vehicle position information of the four sensors includes the vehicle position information P of the INS combined with the inertial navigation GPS =[x GPS ,y GPS ] T , IMU vehicle position information P IMU =[x IMU ,y IMU ] T , vehicle position information P fed back by wire control Kin =[x Kin ,y Kin ] T , the visually recognized vehicle position information P Obs =[x Obs ,y Obs ] T The wire-controlled chassis sensor includes a speed sensor and a rotation angle sensor, which feed back the vehicle's speed and rotation angle. The vehicle kinematic model is combined with the speed and rotation angle to calculate the positioning data P. Kin =[x Kin ,y Kin ] T The visual recognition sensor uses the distance of the static object it detects to reverse the vehicle position information P Obs =[x Obs ,y Obs ] T .

[0012] In another aspect of the present invention, a method for evaluating vehicle operation risk using the autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction comprises the following steps:

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

[0014] Step 2: Perform x on the vehicle position information of the four sensors after time synchronization obtained in step 1. 2 Test, calculate χ 2 Statistics, and preset threshold comparison to make global consistency judgment. If the global anomaly occurs, 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 the vehicle position information of the four sensors and constructs the residual matrix; the residuals are standardized and the local anomaly degree of each sensor is calculated. The motion mode consistency test constructs four vehicle motion models: uniform velocity CV, uniform acceleration CA, coordinated turning CT, and uniform Jerk CJ. The four vehicle motion models interactive multi-model IMM algorithm is run in parallel on the vehicle position information of the four sensors. The probability of each motion model is estimated using the Kalman filter. The JS divergence between the model probability distribution matrices output by each sensor is calculated to construct the motion mode estimation consistency matrix and calculate the local abnormality of each sensor. And output the historical movement pattern, if the local abnormality and If it is 1, it means that there is a sensor whose test result is inconsistent with sensor i. and If it is 2, it means that there are two sensors whose test results are inconsistent with sensor i, and so on;

[0015] Step 3: The local abnormality voting and fault detection counter receives the local abnormality time and local abnormality output from step 2. and If the two consistency checks in step 2 output results that the INS local anomaly degree is greater than 2 and the local anomaly degrees of the remaining sensors are less than 1, it indicates that the four sensors all point to an INS fault, and the INS is confirmed to be faulty. If the voting counter result indicates that the 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 occurs, 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 Er, which is recorded as 0; after the fault occurs, since the INS sensor has been determined to be faulty, the weighted estimated mean of the vehicle position information of the IMU, the wire-controlled chassis, and the visual recognition is used. As the first two dimensions of the state vector, the third dimension is the fault flag Er, which is recorded as 1, and the correction vector u of the vehicle state after the fault moment is obtained t ;

[0017] Step 5: Construct a multi-mode trajectory prediction LSTM network, which includes an LSTM encoder, a classification decoder, and an LSTM decoder. The correction vector u of the vehicle state after the fault moment obtained in step 4 is converted into t The fault feature enhanced trajectory sequence U composed of the fault detection result of INS is input to the LSTM encoder. The classification decoder counts the historical motion patterns output by the motion pattern consistency test in step 2, calculates the probability mean within the historical 0.5 seconds, and splices it with the encoder hidden state to achieve feature enhancement for the hidden layer output of LSTM. It is then input to the classification decoder to output the probability P(m) of 6 types of yaw operations of 3 horizontal and 2 vertical directions. i |X);

[0018] The LSTM decoder combines the hidden layer with the six yaw operation types through the one-hot encoding method, performs six conditional decoding to output the two-dimensional Gaussian distribution parameter Θ under each operation type, and calculates the value of the given operation category m. i The probability distribution P of the future position Y under the historical trajectory X Θ (Y|X,m i );

[0019] According to Bayes' theorem, the given operation category m i The probability distribution P of the future position Y under the historical trajectory X Θ (Y|X,m i ) and the probability P(m) of the yaw operation type i |X) to obtain the fused multimodal conditional probability distribution P(Y|X);

[0020] Step 6: Based on the multimodal conditional probability distribution P(Y|X) in step 5, Monte Carlo sampling and quantization with kinematic constraints are performed. First, the motion pattern is sampled according to the probability distribution of the operation type generated by the classification decoder. Then, the trajectory is generated corresponding to the two-dimensional Gaussian distribution parameter Θ generated by the LSTM decoder. Each trajectory is calculated. After the kinematic constraint is filtered, the proportion of trajectories that meet the kinematic constraints and exceed the safety threshold is counted as the risk value P.risk .

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

[0022] In the above technical solution, the x in step 2 2 The calculation formula of the statistic is:

[0023]

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

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

[0026]

[0027] Where z i-j is the normalized residual vector between different vehicle position information calculated by different sensors, σ i-j The mean of the positioning data of different sensors is statistically calculated for historical data, where i and j are sensors;

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

[0029]

[0030] Where r i-j is the residual between the vehicle position information of different sensors, μ i-j and σ i-j are the mean and standard deviation of historical data statistics; the residual r between vehicle position information of different sensors i-j The calculation formula is:

[0031]

[0032] Where x i ,y i and x j ,y j It is the vehicle position information of different sensors.

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

[0034]

[0035] Where, SP i is the difference measure between the motion mode prediction probability output by sensor i and the average motion mode prediction probability, σ i|j is the total difference variance between the probability matrix of sensor i and the rest of the sensors.

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

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

[0038] Where u t is the correction vector of the vehicle state after the fault, t is the time, t=1,2,…T;

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

[0040]

[0041] Where, t f is the fault moment, Er represents the vehicle fault flag, when there is no fault, it is recorded as 0, otherwise it is recorded as 1, x k ,y k is the vehicle position information of sensor k, is the average value of the vehicle position information estimated by the other three sensors except the INS sensor;

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

[0043]

[0044] Where i is the other three sensors except the INS combined inertial navigation sensor, is the vehicle position information of the IMM algorithm of the i-th sensor except the INS combined inertial navigation sensor.

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

[0046]

[0047] Where, P Θ (Y|X,m i ) is the output of the LSTM decoder for a given operation category m i The probability distribution of the future position Y under the historical trajectory X, Θ is the two-dimensional Gaussian distribution parameter for each time step in the future, P(m i |X) is the probability of the vehicle implementing the i-th operation type output by the classification decoder;

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

[0049]

[0050] Where t is the current time, (t+1) is the next time, and t f Represents the time interval between the fault occurrence and the current time, (t+t f ) is the time corresponding to the fault interval starting from the current time.

[0051] In the above technical solution, the probability P(m i The calculation formula for |X) is:

[0052]

[0053] Where, and are the pth horizontal operation probability and the qth vertical operation probability respectively.

[0054] Compared with the prior art, the present invention has the following beneficial effects:

[0055] 1. The present invention proposes an INS fault detection and risk assessment method that integrates multi-sensor data, an interactive multi-model (IMM) algorithm, a long short-term memory network (LSTM) trajectory prediction, and Monte Carlo sampling. This method constructs a consistency verification framework for multi-source data through time synchronization and kinematic modeling, uses the IMM algorithm to dynamically estimate the probability of vehicle motion patterns, and combines it with the LSTM network to predict the multimodal trajectory distribution under fault conditions. Finally, the proportion of risk trajectories is statistically analyzed through Monte Carlo sampling to output an interpretable safety indicator. Experiments have shown that this method is superior to traditional solutions in terms of fault detection delay, recognition rate, and risk assessment accuracy, and is suitable for enhancing the safety of autonomous driving in complex environments.

[0056] 2. This paper addresses the issue of INS fault detection in autonomous driving systems by innovatively proposing a dual verification mechanism that combines residual consistency at the data level with motion pattern consistency at the model level. This mechanism not only incorporates real-time multi-sensor data consistency verification but also innovatively incorporates an analysis method based on historical motion patterns, significantly improving fault recognition rates while effectively reducing the likelihood of false and missed detections.

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

[0058] 4. Regarding fault response, this invention proposes a complete "detection-assessment-decision" closed-loop solution. By building a multi-modal prediction network that incorporates fault characteristics, it not only identifies faults but also accurately assesses their impact on driving safety, providing comprehensive safety decision-making for autonomous driving systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 Schematic diagram of the autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction of the present invention.

[0060] Figure 2 This is the local anomaly calculation graph based on the IMM algorithm of the present invention.

[0061] Figure 3 This is a flow chart of the IMM filtering algorithm for a single sensor of the present invention. DETAILED DESCRIPTION

[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 only used to explain the present invention and are not intended to limit the present invention.

[0063] Example 1

[0064] like Figure 1 As shown, a motion risk assessment system for an autonomous driving vehicle 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 wire-controlled chassis sensor, and a visual recognition sensor; the INS combined inertial navigation sensor includes GPS and IMU; the verification module includes a χ 2Test, motion pattern consistency test and residual consistency test; 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 the vehicle position information of the four sensors after time synchronization, and inputs it to the inspection module. 2 The test standardizes the residuals and calculates the χ 2 Statistics are compared with the preset threshold to make a global consistency judgment; if the global anomaly occurs, the residual consistency test and motion pattern consistency test are triggered, and the residual consistency test calculates the local anomaly degree of each sensor Movement pattern consistency test calculates the local anomaly of each sensor The local anomaly voting and fault detection counter receives the 2 Local abnormality time and local abnormality degree of inspection and Output INS diagnosis results and INS failure time, and input them into the trajectory correction and feature enhancement module. The trajectory correction and feature enhancement module uses the vehicle position information output by the INS sensor as the vehicle state vector before the failure time according to the INS failure time. After the failure time, since the INS sensor has been determined to be faulty, the position estimation mean of the remaining three sensors is used. As the input state vector, the input state vector adds the fault flag Er to form the correction vector u of the vehicle state after the fault moment. t ; The correction vector u of the vehicle state after the fault moment t The fault feature enhanced trajectory sequence U composed of the fault detection results of INS is input into the LSTM network. The LSTM network outputs the multimodal conditional probability distribution P(Y|X). With the multimodal conditional probability distribution P(Y|X) as input, the Monte Carlo sampling method is used to generate N sampling trajectories, which are recorded as N total After all sampled trajectories are screened by kinematic constraints, the trajectories N that meet the kinematic constraints and exceed the safety threshold are counted. out_of_range Proportion as risk value P risk .

[0066] Example 2

[0067] The method for performing vehicle motion risk assessment using the autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction in 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 the vehicle's two-dimensional coordinates in UTM coordinates. Combined with the vehicle's drive-by-wire chassis sensor, inertial navigation (IMU) sensor, and visual recognition sensor, calculate the vehicle position information of the four sensors. The drive-by-wire chassis sensor uses the vehicle's kinematic model combined with vehicle speed and turning angle to calculate the vehicle position information. The visual recognition sensor uses the distance to static objects it detects to inversely resolve the vehicle position information. The INS vehicle position information and the IMU vehicle position information are directly output by the INS. These asynchronous vehicle position information are time-synchronized to obtain the vehicle position information of the four sensors in a unified data format.

[0069] The vehicle position information of the four sensors includes the vehicle position information P of the INS combined inertial navigation GPS =[x GPS ,y GPS ] T , IMU vehicle position information P IMU =[x IMU ,y IMU ] T , vehicle position information P fed back by wire control Kin =[x Kin ,y Kin ] T , the visually recognized vehicle position information P obs =[x obs ,y obs ] T The wire-controlled chassis sensor includes a speed sensor and a rotation angle sensor, which feed back the vehicle's speed and rotation angle. The vehicle kinematic model is combined with the speed and rotation angle to calculate the positioning data P. Kin =[x Kin ,y Kin ] T The visual recognition sensor uses the distance of the static object it detects to reverse the vehicle 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 angle θ fed back by the vehicle chassis controller area network (CAN bus), and the position information (distance, angle) of static obstacles (such as street lights and traffic signs) identified by the vision system relative to the vehicle.

[0071] Through actual measurements, the data transmission periods of each sensor are: INS (20ms), IMU (20ms), chassis feedback (20ms), and visual recognition (100ms). Since visual recognition has the lowest sampling period, this paper sets the visual recognition sampling period of 100ms as the system baseline time axis, and requires interpolation calculation of high-frequency sensor (INS / IMU / chassis) data to achieve time alignment.

[0072] Use interpolation to synchronize frequency: When there is historical data of high-frequency sensors at the reference time, the second-order Lagrange interpolation method is used for interpolation. For example, at the 100ms reference time, the INS observation value can be calculated using its 80ms, 100ms, and 120ms measurements:

[0073]

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

[0075]

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

[0077] The method for calculating the vehicle position information of the visual recognition sensor is specifically 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 is steered, and the front wheel is driven; the vehicle speed (V) is the speed of the rear axle center along the longitudinal axis of the vehicle; dynamic factors such as tire slip and suspension influence are ignored, and the 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 coordinates of the rear axle center (x, y); Vehicle coordinate system: x'-y', the origin is at the rear axle center, and the x' axis is along the longitudinal axis of the vehicle; Heading angle: ψ, the angle between the vehicle longitudinal axis and the global X axis (counterclockwise is positive); State variables are x, y, ψ (position and heading angle); Input V (vehicle speed), θ (front wheel angle).

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

[0081]

[0082] The differential equations for lateral and longitudinal motion x, y are:

[0083]

[0084] Substituting the lateral and longitudinal x, y motion differential equations into the heading angle change rate differential equation yields the following differential equations, which gives the complete kinematic model:

[0085]

[0086] The vehicle speed V and steering angle θ are discretized data obtained by the wire control system. Therefore, the complete kinematic model is also discretized and deformed. Assuming the sampling time is Δt, the Euler method is used for discretization, and the following formula is obtained:

[0087]

[0088] Assume that the historical position information at the moment of 3s is x0, y0.

[0089] Integrating the complete kinematic model after discretization deformation yields:

[0090]

[0091] By using visual recognition results to solve the vehicle's current position, and leveraging the prior knowledge that static objects (such as road signs and traffic lights, common static objects in urban environments) remain unchanged in the geodetic coordinate system, the vehicle's position changes within the past 5 seconds are calculated in real time.

[0092] The algorithm maintains a sliding window of 5 seconds in length. Assume that the UTM coordinates of the vehicle at the historical time t0 are (x0, y0), the heading angle is θ0, and a static obstacle is selected as the reference object for solution. The relative coordinates in the vehicle coordinate system are Through rotation transformation and translation operations, it can be mapped to the UTM coordinate system to obtain the position of the reference object in the UTM coordinate system:

[0093]

[0094] Where R(θ0) is the rotation matrix between the vehicle coordinate system and the earth coordinate system. In the two-dimensional plane positioning problem, the vehicle coordinate system is set to V, whose origin coincides with the center of mass of the vehicle, the x-axis points in the direction of vehicle movement, and the y-axis extends perpendicular to the x-axis to the left. The global coordinate system uses the UTM coordinate system W.

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

[0096]

[0097] Since the static characteristics of the obstacle are known as prior knowledge, X obs With Y obsKeep it constant in the subsequent derivation, so that the vehicle position within any valid detection time can be calculated. At any time t, we still have:

[0098]

[0099] Where x t is the UTM coordinate position of the vehicle at time t, θ t are the longitudinal distance, lateral distance and yaw angle of the reference object output by the visual recognition at time t, and are obtained jointly:

[0100]

[0101] Considering that static obstacles may disappear temporarily due to occlusion or exceeding the sensor range within the 5-second time window of the autonomous vehicle, the system ensures continuous solution according to the following rules, builds a historical data pool, and stores the type, ID, relative coordinates and vehicle heading angle of all static obstacles in the past 5 seconds, forming a sliding time window.

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

[0103] Failure judgment: If the solution reference object is beyond the sensor's field of view or there is no valid recognition data for two consecutive time steps, switch to a new solution reference object;

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

[0105] Loop recursion: The calculated position of the original reference obstacle at the last valid moment is used as the initial value of the next loop and Combine the newly selected static obstacle data and recursively calculate to the current moment

[0106] The vehicle position information at the current moment calculated by visual recognition is:

[0107]

[0108] Where, θ k Indicates the horizontal and vertical distances and camera heading angles at the end of the effective time of the i-th solution reference object

[0109] If there is no available reference at the initialization time or there is a subinterval without any available reference information within the 5-second sliding window, the fault detection algorithm relying on this method is suspended.

[0110] Step 2: Perform x on the vehicle position information of the four sensors obtained in step 1. 2 Test, calculate χ 2 Statistics, and preset threshold comparison to make global consistency judgment. If the global anomaly is found, 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 the vehicle position information of the four sensors and constructs the residual matrix; the residuals are 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), coordinated turning (CT), and uniform jerk (CJ). Four interactive multi-model IMM algorithms are run in parallel on the vehicle position information of four sensors. The probability of each motion model is estimated using a Kalman filter. The JS divergence between the model probability distribution matrices output by each sensor is statistically analyzed to construct a motion pattern estimation consistency matrix. The local anomaly degree of each sensor is calculated. And output historical movement patterns;

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

[0112] First, construct the residual matrix, assuming that the outputs of the four sensors at the same time are: INS vehicle position information: P GPS =[x GPS ,y GPS ] T ; IMU vehicle position information: P IMU =[x IMU ,y IMU ] T ; Wire control feedback vehicle position information: P Kin =[x Kin ,y Kin ] T ; Visual recognition of vehicle position information: P Obs =[x Obs ,y Obs ] T .

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

[0114]

[0115] The residual matrix R is defined as the Euclidean distance between the position difference of each sensor and the rest of the sensors:

[0116]

[0117] At the same time, in order to ensure the rapid response of the fault diagnosis system, a hierarchical voting mechanism is established for the residual consistency test, and the χ 2 detector and define the normalized residual vector z:

[0118]

[0119] Where μ i-j and σ i-j is the mean and standard deviation of historical data statistics, which characterizes the distribution characteristics of the residual under fault-free conditions.

[0120] Construct χ 2 Statistics are used to comprehensively evaluate the consistency of multi-sensor residuals:

[0121]

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

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

[0124] If χ 2 >γ, the consistency exception flag is triggered. Further combined with the hierarchical voting mechanism:

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

[0126] Secondary positioning: Count the number of abnormal residual pairs of each sensor and define the local abnormality degree

[0127]

[0128] Where z i-j is the normalized residual vector between different vehicle position information calculated by different sensors, σ i-j The mean of the positioning data of different sensors is statistically calculated for historical data, where i and j are sensors;

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

[0130] Define a set of motion models that can describe common vehicle motion states. This invention uses the following four:

[0131] (a) Uniform motion model (CV): The CV model assumes that the target's speed in the horizontal and vertical directions will be constant over a period of time in the future, maintaining a uniform linear motion state. However, due to the existence of various disturbances in reality, the target will be disturbed by various external forces, resulting in fluctuations in the target's acceleration. The acceleration effect of the disturbance force 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 under the uniform motion model of the object.

[0132]

[0133] The state of the target at time k is described by the uniform motion model, x k ,y k , They are the horizontal and vertical coordinates and acceleration of the target, respectively, and the discrete form of the state space equation of the uniform motion model motion process It can be expressed as:

[0134]

[0135] Where W k is Gaussian white noise with zero mean, F CV , G CV are the transition matrix and noise driving matrix of the CV model, which are:

[0136]

[0137] Where T is the time interval.

[0138] (b) Uniform Acceleration Model (CA): The CA model assumes that the acceleration of the target in the horizontal and vertical directions will be constant in the future period, and the target will maintain a uniformly accelerated linear motion state. Similarly, due to the external disturbances in the horizontal and vertical directions, the extra accelerations in the horizontal and vertical directions caused by the disturbance force are regarded as independent Gaussian white noises, and the state of the target under the uniform acceleration model is calculated. To model:

[0139]

[0140] is the state of the target under the uniform acceleration motion model, They are the acceleration of the target in the horizontal and vertical directions, and the discrete form of the state space equation of the uniform acceleration process It can be expressed as:

[0141]

[0142] Where W k is the noise matrix, which is Gaussian white noise with a mean of 0, indicating the interference to the acceleration of the object, F CA , G CA are the transition matrix and noise driving matrix of the uniform acceleration model, which are:

[0143]

[0144] (c) Cooperative Turning Motion Model (CT): The CT model assumes that the target will perform circular motion in the future with no change in angular velocity, direction, or magnitude. Unlike the uniform velocity and uniform acceleration models, which are linear models, the CT model is nonlinear and needs to be linearized before being converted into a discrete state space equation. The descriptive vector of the target cooperative turning motion in the CT model is constructed as

[0145]

[0146] Where, is the description vector of the target coordinated turning motion. When the coordinated turning motion is performed in a two-dimensional plane, its real-time position in the global coordinate is:

[0147]

[0148] Where x c ,y c is the center of the target circular motion, is the initial angle of the target, and ω is the angular velocity of the target.

[0149] Then the discrete state space equation of the CT model motion process is It can be expressed as

[0150]

[0151] Where, F TT is the transition matrix of the cooperative turning motion model, is the description vector of the target cooperative turning motion, G CT is the noise driving matrix of the cooperative turning motion model, W k is Gaussian white noise with zero mean.

[0152] Transition matrix F of cooperative turning motion model CT Expressed as:

[0153]

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

[0155]

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

[0157]

[0158] Discrete form of the state space equation of uniform jerk motion process It can be expressed as:

[0159]

[0160] Where, F CJ , G CJ are the transition matrix and noise driving matrix of the uniform acceleration model, which can be expressed in the form of block matrices as follows:

[0161]

[0162] Same as above, the noise matrix is ​​W k , which is a Gaussian white noise with a mean of 0, indicating the interference of the jerk value of the object.

[0163] like Figure 2 、 Figure 3 As shown, an IMM filter is independently run on the positioning data of each sensor, and each IMM filter contains the 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 target state estimation of model j at time k-1 has been obtained at the beginning of the algorithm Covariance Model probability And the state transfer matrix P. The calculation process is as follows: The mixing probability of motion model i to motion model j

[0165]

[0166]

[0167] Where, is the predicted probability of model j, which represents the probability that the target is in model j after input interaction, p ij is the transition probability from model i to model j, The probability of model i at time k-1. The mixed state estimate of model j at time k-1 for:

[0168]

[0169] Where, is the estimated target state of model i at time k-1, is the mixing probability from motion model i to motion model j.

[0170] Mixed covariance estimate for model j for:

[0171]

[0172] The filter used in parallel filtering is Kalman filtering. First, the positioning data obtained by a certain sensor will be input into four Kalman filters respectively. The prediction models of the four Kalman filters are the CV, CA, CT, and CJ models mentioned in the previous section. j For example, the formula is as follows:

[0173] The state prediction equation is consistent with the discrete form of the state space equation of the model:

[0174]

[0175] is the prior prediction value obtained based on the optimal estimate in the previous step, is the state transfer matrix of motion model j at time k-1, is the initial predicted state obtained at time k-1 based on the information at time k-2 under motion model j, G j is the process noise driving matrix of motion model j, W k is the process noise vector at time k, so G j W k is the state change amount introduced by process noise for motion model j at time k.

[0176] Its error covariance predicted value

[0177]

[0178] Where, is the state transfer matrix of motion model j at time k-1, is the initial state estimation covariance matrix of motion model j at time k-1 based on the information at time k-1, is the state transfer matrix of motion model j at time k-1 The transposed matrix, G j is the process noise driving matrix of motion model j, Q j is the process noise covariance matrix of motion model j, is the incremental part of the state covariance introduced by the process noise of the motion model j.

[0179] The Kalman gain is:

[0180]

[0181] Where H j is the observation matrix, is the observation matrix h j The transposed matrix of , R is the observation noise covariance matrix, is the error covariance predicted value.

[0182] state Updates and Covariance The update formula is:

[0183]

[0184] Where, is the prior prediction value obtained based on the optimal estimate of the previous step (k-1 moment), is the Kalman gain, Z k is the observed value at time k, H j is the observation matrix, I is the identity matrix, and the third step is the model probability update: the likelihood function is used to update the probability of model j

[0185]

[0186] Where c is the normalization constant, The likelihood function of model j is

[0187]

[0188] Where, is the distance between the observed value of the j-th model at time k and its predicted value at the previous time, called the residual. is the covariance matrix of the residuals.

[0189]

[0190] Finally, the probabilities of the four updated models are combined into a 4*1 probability matrix Mi At the kth moment, the recognition estimation value M of sensor i for the four motion models i (k) is expressed as:

[0191]

[0192] In order to measure the consistency of different sensors’ judgments on the current motion mode, 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] Where 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 Courbet-Lébouler divergence between 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 and the mean probability vector A.

[0195]

[0196] Where M ix is the model probability output by the x-th Kalman filter in the interactive multimodal algorithm of sensor i.

[0197] According to the above steps, the four sensors are grouped in pairs to perform JS divergence calculation. In order to facilitate the calculation, JS(M i ,M i )=0, that is, the JS divergence between the sensor and its own probability matrix is ​​0. Finally, the motion pattern estimation consistency matrix P is obtained consist :

[0198]

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

[0200]

[0201] Where, SP i is the difference measure between the motion mode prediction probability output by sensor i and the average motion mode prediction probability, σ i|jis the total difference variance between the probability matrix of sensor i and the rest of the sensors.

[0202] Step 3: The local abnormality voting and fault detection counter receives the local abnormality time and local abnormality C output in step 2. i and If the two consistency checks in step 2 output results that the INS local anomaly degree is greater than 2 and the local anomaly degrees of the remaining sensors are less than 1, it indicates that the four sensors all point to an INS fault, and the INS is confirmed to be faulty. If the voting counter result indicates that the 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 combines the test results of the previous two steps to make a final INS fault diagnosis, determine the exact time when the INS fault occurred, and compare the preliminary fault judgment results of residual consistency and motion pattern consistency. If both steps determine that the same sensor (such as INS) is faulty, then the sensor is finally confirmed to be faulty. Output the diagnostic result: "INS fault". If only one step determines that a sensor is faulty, or the faulty sensors determined in the two steps are different, or multiple sensors are judged to be faulty, they can be handled according to the preset strategy, such as being marked as "uncertain" or "multiple positioning system faults". If both steps determine that the system is consistent, the diagnostic result is: "All positioning systems are consistent."

[0204] In order to make the fault detection algorithm take into account the characteristics of multiple sensors, response delay and false alarm rate, an abnormality fault detection counter is designed. The abnormality fault detection counter only maintains a counter Vi for the INS, with an initial value of 0. The counting update logic mechanism is as follows:

[0205] When the INS combined inertial navigation sensor satisfies the following four inequalities at the same time, the counter Vi increases by 10 units, otherwise Vi decreases by 1 unit:

[0206]

[0207] The fault onset time is determined based on the results of the motion pattern consistency check (step 3), as it reflects persistent inconsistencies at the motion model level. The timestamp of the first sustained increase in the local abnormality of the motion pattern of the sensor (e.g., GPS) ultimately diagnosed as faulty is recorded. This time is the output fault onset time.

[0208] Step 4: Based on the INS fault occurrence time output in step 3, before the fault occurs, 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 Er, which is recorded as 0; after the fault occurs, since the INS sensor has been determined to be faulty, the weighted estimated mean of the vehicle position information of the IMU, the wire-controlled chassis, and the visual recognition is used. As the first two dimensions of the state vector, the third dimension is the fault flag Er, which is recorded as 1, and the correction vector u of the vehicle state after the fault moment is obtained t Specifically:

[0209] The INS data that may be disturbed is corrected according to the diagnosis results, and a comprehensive state vector including position, motion mode and fault status is constructed as the input of the subsequent prediction model.

[0210] Before the fault time, the position output by the GPS system and the output of the IMM algorithm are combined into a new vehicle state vector. After the fault time, the GPS data is invalid, so the position estimate mean of the redundant sensors is used instead. The fault time is recorded as t f , then the vehicle state vector u t Corrected to:

[0211]

[0212] Where x k ,y k is the vehicle position information when the INS sensor is fault-free, Er is the vehicle fault flag, which is 0 when there is no fault, otherwise it is 1, k is the time, t f The failure time.

[0213] is the average value of the vehicle position information estimated by the other three sensors except the INS sensor, and its calculation formula is:

[0214]

[0215] Step 5: Use the long short-term memory network (LSTM) to learn the pattern of the historical state sequence 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 convert the correction vector u of the vehicle state after the fault moment obtained in step 4 into tThe fault feature enhanced trajectory sequence U composed of the fault detection result of INS is input to the LSTM encoder. The classification decoder counts the historical motion patterns output by the motion pattern consistency test in step 2, calculates the probability mean within the historical 0.5 seconds, and splices it with the encoder hidden state to achieve feature enhancement for the hidden layer output of LSTM. It is then input to the classification decoder to output the probability P(m) of 6 types of yaw operations of 3 horizontal and 2 vertical directions. i |X);

[0217] The LSTM decoder combines the hidden layer with the six yaw operation types through the one-hot encoding method, performs six conditional decoding to output the two-dimensional Gaussian distribution parameter Θ under each operation type, and calculates the value of the given operation category m. i The probability distribution P of the future position Y under the historical trajectory X Θ (Y|X,m i );

[0218] According to Bayes' theorem, the given operation category m i The probability distribution P of the future position Y under the historical trajectory X Θ (Y|X,m i ) and the probability P(m) of the yaw operation type i |X) to obtain the fused multimodal conditional probability distribution P(Y|X);

[0219] In the LSTM network, 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 has a more explicit relationship with the operation type, feature enhancement design is performed at the classification decoding.

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

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

[0222]

[0223] Among them, Er represents the vehicle fault flag. When there is no fault, it is recorded as 0, otherwise it is recorded as 1. It is the position estimate of the other three sensors except the INS sensor.

[0224] The above time series data is passed through 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. The input to the trajectory parameter decoder of the LSTM decoder is the hidden layer state output henc. After one-hot encoding and embedding, the LSTM decoder calculates the two-dimensional Gaussian distribution parameters Θ of the vehicle in the future time T′. This paper assumes that the motion distribution under each operation type conforms to an independent bivariate Gaussian distribution. For each operation type, four two-dimensional Gaussian parameters are output, ignoring covariance. The calculation formula for the two-dimensional Gaussian distribution parameters Θ of the vehicle in the future time T′ (defined as the time from the current moment to the future moment corresponding to the occurrence of the fault) is:

[0225]

[0226]

[0227] Where Θ is the two-dimensional Gaussian distribution parameter of the vehicle in the future T′ time. In the context of this algorithm, t is the current time, and t+1 is the next time t f Represents the time interval between the fault occurrence and the current time, t+t f is the time corresponding to the fault interval from the current time, μ x 、μ y are the means in the x and y directions, σ x , σ y are the variances in the x and y directions respectively. According to the two-dimensional Gaussian distribution parameter Θ of the vehicle in a given operation category m i Calculate the probability distribution P of the future position Y based on the historical trajectory X Θ (Y|X,m i ).

[0228] Since the motion pattern has a more explicit relationship with the operation type, the present invention performs feature enhancement design in the classification decoder, and takes the average motion pattern probability of the historical 0.5s for each time step (0.1s)

[0229]

[0230] Where, is the probability of the kth motion mode, k is the number of time steps, i is the i-th motion mode, where, The calculation formula is:

[0231]

[0232] Where, is the average value of the CV motion model probability in the IMM results of the vehicle position information of the other three sensors except the INS sensor:

[0233]

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

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

[0236]

[0237] This vector is used as the input of the classification decoder, combining three horizontal operations and two vertical operations, and outputting each operation type m in the six operation types through the fully connected layer + softmax. i The probability P(m i |X), which is calculated as follows:

[0238]

[0239] Among them, P(m i |X) is the probability of the vehicle implementing the i-th operation type, and is the pth horizontal operation probability and the qth vertical operation probability.

[0240] According to the intuitive performance of simulation experiments, the present invention designs 6 operation types for vehicle loss of 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 risk of accelerated movement of a vehicle in a fault state is generally higher than that of decelerated and decelerated movement, the present invention defines that when the average speed of the vehicle within 0.5s at a certain time step changes by more than 80% relative to the previous time step, it is an accelerated driving operation; otherwise, it is a uniform or decelerated movement.

[0242] For lateral motion, the present invention defines that when the lateral deviation of the vehicle position from the reference line is greater than 0.5 meters to the left, the operation category of this time step is left yaw, right yaw and so on. The operation category of the remaining time steps is defined as lane keeping.

[0243] The goal of using an LSTM network is to predict the future location of the vehicle and output it in the form of 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] Where, P Θ (Y|X,m i ) is in a given operation category m i The probability distribution of the future position Y under the condition of the historical trajectory X, P(m i |X) is the operation category belonging to m under the given historical accumulation history trajectory X i The probability of i is one of the six operation categories, m1 is accelerated left yaw, m2 is accelerated right yaw, and so on.

[0246] Step 6: Based on the multimodal conditional probability distribution P(Y|X) in step 5, Monte Carlo sampling and quantization with kinematic constraints are performed. First, the motion pattern is sampled according to the probability distribution of the operation type generated by the classification decoder. Then, the trajectory is generated corresponding to the two-dimensional Gaussian distribution parameter Θ generated by the LSTM decoder. Each trajectory is calculated. After the kinematic constraint is filtered, the proportion of trajectories that meet the kinematic constraints and exceed the safety threshold is counted as the risk value P. risk .

[0247] Monte Carlo sampling is used to quantify risk. Monte Carlo sampling is a numerical calculation method based on random sampling, used to approximate solutions to complex problems, such as integrals, expectations, and probability distributions. Its core concept is to generate a large number of random samples and apply statistical principles to estimate the solution to the problem. The results improve with the number of samples, making it well-suited for solving problems that cannot be directly solved 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 uniformly distributed between [0, 1] is used to select the operation type used for this sampling.

[0252] 2. Trajectory sampling and splicing: Discretize time for sampling, set the discrete step length to 0.1s. Since the vehicle positions conform to the two-dimensional normal distribution and are independent of each other, each time step has

[0253]

[0254] Therefore, we can directly use the Gaussian random number generator to obtain it and splice it into a complete trajectory from t∈[0,1].

[0255] 3. Repeat the above steps to obtain N sampling trajectories, recorded as N total Since the assessment of vehicle operation risk is involved, the present invention limits the running time of Monte Carlo sampling and terminates the sampling when the number of samples reaches 1000 or when the sampling time exceeds 0.5s.

[0256] 4. Finally, all sampled trajectories are eliminated according to kinematic constraints, and the maximum and minimum values ​​of the turning radius and acceleration of each trajectory are tested using the sliding window method, with a sliding window length of 1s.

[0257] 5. Risk Quantification Calculation

[0258] After sampling and generating multi-modal trajectories and selecting trajectories that meet kinematic constraints, a safety assessment is required for each trajectory to quantify risk. This is essentially a qualitative assessment and statistics of the sampled data, which is divided into two steps: safety threshold determination and risk statistics. The safety threshold determination has two criteria:

[0259] The vehicle crosses the lane line or the deviation distance is too large: According to the Chinese road standard (JTGB01-2014), the width of a general urban lane is 3.5 meters. When the farthest end of the vehicle is 1.75 meters away from the center of the lane, it is considered to have crossed the lane line. The deviation distance threshold is set to the same threshold with reference to this regulation. The invention adopts a sliding window mechanism with a duration of 0.5s to determine whether each trajectory is a risk trajectory. Specifically, the sliding window starts at t = 0 and slides backward over time. When the vehicle's running direction deviates significantly, that is, the heading angle deviation between the vehicle trajectory and the target trajectory exceeds a pre-set reasonable threshold, the trajectory within the sliding window will be determined as a risk trajectory. The number of all risk trajectories obtained through the above determination is recorded as N. out_of_range , the risk value of the vehicle operation at the current moment can be calculated

[0260] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A motion risk assessment system for autonomous driving vehicles based on multimodal trajectory prediction, characterized in that: The system 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 wire-controlled chassis sensor, and a visual recognition sensor; the INS combined inertial navigation sensor includes GPS and IMU; the verification module includes a χ 2 Test, motion pattern consistency test and residual consistency test; the LSTM network includes an LSTM encoder, a classification decoder and an LSTM decoder; The time synchronization module receives asynchronous vehicle position information from four sensors, outputs the vehicle position information of the four sensors after time synchronization, and inputs it to the inspection module. 2 The test standardizes the residuals and calculates the χ 2 Statistics, compared with preset thresholds to make global consistency judgments; If the global anomaly occurs, the residual consistency test and motion pattern consistency test are triggered. The residual consistency test calculates the local anomaly degree of each sensor. Movement pattern consistency test calculates the local anomaly of each sensor The local anomaly voting and fault detection counter receives the 2 Local abnormality time and local abnormality degree of inspection and Output INS diagnosis results and INS failure time, and input them into the trajectory correction and feature enhancement module. The trajectory correction and feature enhancement module uses the vehicle position information output by the INS sensor as the vehicle state vector before the failure time according to the INS failure time. After the failure time, since the INS sensor has been determined to be faulty, the position estimation mean of the remaining three sensors is used. As the input state vector, the input state vector adds the fault flag Er to form the correction vector u of the vehicle state after the fault moment. t ; The correction vector u of the vehicle state after the fault moment t The fault feature enhanced trajectory sequence U composed of the fault detection results of INS is input into the LSTM network. The LSTM network outputs the multimodal conditional probability distribution P(Y|X). With the multimodal conditional probability distribution P(Y|X) as input, the Monte Carlo sampling method is used to generate N sampling trajectories, which are recorded as N total After all sampled trajectories are screened by kinematic constraints, the trajectories N that meet the kinematic constraints and exceed the safety threshold are counted. out_of_range Proportion as risk value 2. The autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction according to claim 1, characterized in that: The vehicle position information of the four sensors includes the vehicle position information P of the INS combined inertial navigation GPS =[x GPS ,y GPS ] T , IMU vehicle position information P IMU =[x IMU ,y IMU ] T , vehicle position information P fed back by wire control Kin =[x Kin ,y Kin ] T , the visually recognized vehicle position information P Obs =[x Obs ,y Obs ] T The wire-controlled chassis sensor includes a speed sensor and a rotation angle sensor, which feed back the vehicle's speed and rotation angle. The vehicle kinematic model is combined with the speed and rotation angle to calculate the positioning data P. Kin =[x Kin ,y Kin ] T The visual recognition sensor uses the distance of the static object it detects to reverse the vehicle position information P Obs =[x Obs ,y Obs ] T .

3. A method for evaluating vehicle operation risk using the autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction according to claim 1, characterized in that: The following steps are involved: Step 1: Acquire vehicle position information from the vehicle's INS combined inertial navigation sensor, wire-controlled chassis sensor, inertial navigation IMU sensor, and visual recognition sensor. The wire-controlled chassis sensor uses a vehicle kinematic model combined with vehicle speed and turning angle to calculate vehicle position information. The visual recognition sensor uses the distance to static objects it detects to inversely resolve vehicle position information. The INS vehicle position information and the IMU vehicle position information are directly output by the INS. The asynchronous vehicle position information of the four sensors is time-synchronized to obtain the vehicle position information of the four sensors in a unified data format after time synchronization. Step 2: Perform x on the vehicle position information of the four sensors after time synchronization obtained in step 1. 2 Test, calculate χ 2 Statistics, and preset threshold comparison to make global consistency judgment. If there is global abnormality, the global abnormality 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 residual between the vehicle position information of the four sensors and constructs the residual matrix; Normalize the residuals and calculate the local anomaly of each sensor The motion pattern consistency test constructs four vehicle motion models: uniform velocity CV, uniform acceleration CA, coordinated turning CT, and uniform jerk CJ. The four vehicle motion models' interactive multi-model IMM algorithm is run in parallel on the vehicle position information of the four sensors. The Kalman filter is used to estimate the probability of each motion model. 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 calculated. And output the historical movement pattern, if the local abnormality and If it is 1, it means that there is a sensor whose test result is inconsistent with sensor i. and If it is 2, it means that there are two sensors whose test results are inconsistent with sensor i, and so on; Step 3: The local abnormality voting and fault detection counter receives the local abnormality time and local abnormality output from step 2. and If the two consistency checks in step 2 output results that the INS local anomaly degree is greater than 2 and the local anomaly degrees of the remaining sensors are less than 1, it indicates that the four sensors all point to an INS fault, and the INS is confirmed to be faulty. If the voting counter result indicates that the 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 occurs, 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 Er, which is recorded as 0; after the fault occurs, since the INS sensor has been determined to be faulty, the weighted estimated mean of the vehicle position information of the IMU, the wire-controlled chassis, and the visual recognition is used. As the first two dimensions of the state vector, the third dimension is the fault flag Er, which is recorded as 1, and the correction vector u of the vehicle state after the fault moment is obtained t ; Step 5: Construct a multi-mode trajectory prediction LSTM network, which includes an LSTM encoder, a classification decoder, and an LSTM decoder. The correction vector u of the vehicle state after the fault moment obtained in step 4 is converted into t The fault feature enhanced trajectory sequence U composed of the fault detection result of INS is input to the LSTM encoder. The classification decoder counts the historical motion patterns output by the motion pattern consistency test in step 2, calculates the probability mean within the historical 0.5 seconds, and splices it with the encoder hidden state to achieve feature enhancement for the hidden layer output of LSTM. It is then input to the classification decoder to output the probability P(m) of 6 types of yaw operations of 3 horizontal and 2 vertical directions. i |X); The LSTM decoder combines the hidden layer with the six yaw operation types through the one-hot encoding method, performs six conditional decoding to output the two-dimensional Gaussian distribution parameter Θ under each operation type, and calculates the value of the given operation category m. i The probability distribution P of the future position Y under the historical trajectory X Θ (Y|X,m i ); According to Bayes' theorem, the given operation category m i The probability distribution P of the future position Y under the historical trajectory X Θ (Y|X,m i ) and the probability P(m) of the yaw operation type i |X) to obtain the fused multimodal conditional probability distribution P(Y|X); Step 6: Based on the multimodal conditional probability distribution P(Y|X) in step 5, Monte Carlo sampling and quantization with kinematic constraints are performed. First, the motion pattern is sampled according to the probability distribution of the operation type generated by the classification decoder. Then, the trajectory is generated corresponding to the two-dimensional Gaussian distribution parameter Θ generated by the LSTM decoder. Each trajectory is calculated. After the kinematic constraint is filtered, the proportion of trajectories that meet the kinematic constraints and exceed the safety threshold is counted as the risk value P. risk .

4. The method for assessing motion risk of an autonomous driving vehicle based on multimodal trajectory prediction according to claim 3, wherein: In step 1, interpolation and time-shift interpolation methods are used to synchronize the asynchronous positioning data of the four positioning sources.

5. The autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction according to claim 3, characterized in that: Step 2 2 The calculation formula of the statistic is: Where N pairs is the number of sensor pairs, i.e. 2 Degrees of freedom of the distribution, N pairs =6, is the square of the normalized residual vector of the k-th sensor pair, and k is the number of the k-th sensor pair.

6. The autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction according to claim 3, characterized in that: The local anomaly degree obtained by the residual consistency test in step 2 The calculation formula is: Where z i-j is the normalized residual vector between different vehicle position information calculated by different sensors, σ i-j The mean of the positioning data of different sensors is statistically calculated for historical data, where i and j are sensors; The normalized residual vector z between different vehicle position information calculated by different sensors i-j The calculation formula is: Where r i-j is the residual between the vehicle position information of different sensors, μ i-j and σ i-j are the mean and standard deviation of historical data statistics; the residual r between vehicle position information of different sensors i-j The calculation formula is: Where x i ,y i and x j ,y j It is the vehicle position information of different sensors.

7. The autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction as claimed in claim 3, characterized in that: The local abnormality obtained by the motion pattern consistency test in step 2 The calculation formula is: Where, SP i is the difference measure between the motion mode prediction probability output by sensor i and the average motion mode prediction probability, σ i|j is the total difference variance between the probability matrix of sensor i and the rest of the sensors.

8. The autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction according to claim 3, characterized in that: The calculation formula of the fault feature enhanced trajectory sequence U of the trajectory correction and feature enhancement module after the fault time in step 5 is: U={u1,u 2, u t ,…,u T } Where u t is the correction vector of the vehicle state after the fault, t is the time, t=1,2,…T; Correction vector u of vehicle state after fault t The calculation formula is: Where, t f is the fault moment, Er represents the vehicle fault flag, when there is no fault, it is recorded as 0, otherwise it is recorded as 1, x k ,y k is the vehicle position information of sensor k, is the average value of the vehicle position information estimated by the other three sensors except the INS sensor; The average value of the vehicle position information estimated by the other three sensors except the INS sensor The calculation formula is: Where i is the other three sensors except the INS combined inertial navigation sensor, is the vehicle position information of the IMM algorithm of the i-th sensor except the INS combined inertial navigation sensor.

9. The autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction according to claim 3, characterized in that: The calculation formula for the multimodal conditional probability distribution P(Y|X) output by the LSTM network in step 5 is: Where, P Θ (Y|X,m i ) is the output of the LSTM decoder for a given operation category m i The probability distribution of the future position Y under the historical trajectory X, Θ is the two-dimensional Gaussian distribution parameter for each time step in the future, P(m i |X) is the probability of the vehicle implementing the i-th operation type output by the classification decoder; The calculation formula for the two-dimensional Gaussian distribution parameter Θ at each future time step is: Where t is the current time, (t+1) is the next time, and t f Represents the time interval between the fault occurrence and the current time, (t+t f ) is the time corresponding to the fault interval starting from the current time.

10. The autonomous driving vehicle motion risk assessment system based on multi-modal trajectory prediction according to claim 9, characterized in that: The probability P(m) that the vehicle performs the i-th operation type i The calculation formula for |X) is: Where, and are the pth horizontal operation probability and the qth vertical operation probability respectively.

Images