Rail transit obstacle multi-source sensor sensing result fusion method
By assigning an independent Kalman filter to each obstacle in rail transit and establishing a time-series observation queue, the problem of the timing of observations in multi-sensor systems is solved, the stability and accuracy of obstacle state estimation are achieved, and the safety of trains is ensured.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-12-24
- Publication Date
- 2026-03-27
AI Technical Summary
In obstacle tracking, the timing of observations in a multi-sensor system cannot be guaranteed, resulting in non-timing of the observations input to the Kalman filter, which affects the accuracy of obstacle state estimation.
A multi-source sensor data processing method based on the vehicle coordinate system is adopted. By assigning an independent Kalman filter to each obstacle, a time-series observation queue is established to dynamically manage the multi-source sensor data. The time-series queue and Kalman filter are used for state updates and matching to solve the problem of asynchronous time-series data of multiple sensors.
It achieves stability and accuracy in obstacle state estimation in multi-sensor systems, avoids the impact of real-time filtering fluctuations on braking decisions, adapts to the perception accuracy of different sensors, and ensures train safety.
Smart Images
Figure CN121739999A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of computer vision technology, and in particular to a method for fusing perception results from multiple sources of sensors for obstacles in rail transit. Background Technology
[0002] In the field of computer vision, using Kalman filters to denoise and filter targets is a common method for tracking obstacles.
[0003] The formula for the Kalman filter is as follows:
[0004] (Formula 1)
[0005] (Formula 2)
[0006] (Formula 3)
[0007] (Formula 4)
[0008] (Formula 5)
[0009] In the above formula, Indicates the filter in The state quantity at time step is also called the posterior state estimate. In the field of obstacle tracking, That is to say, the object is in The three-dimensional spatial state information at any given time includes: the x, y, and z coordinates of the object in three-dimensional space, and its velocity ( , ), facing angle And the length, width, and height of the target.
[0010] Right now:
[0011]
[0012] Indicates the filter in Prior state estimation at time t, i.e., based on The filter inferred from the state at time t is The predicted value at any given time.
[0013] matrix This represents the state transition matrix. In the field of obstacle tracking, it is assumed that... Time and The time difference of time is Then we have:
[0014]
[0015] This represents the control input of the filter, which is usually set to zero in the field of obstacle tracking.
[0016] This represents the control input matrix, which is also set to zero in the field of obstacle tracking.
[0017] Indicates the filter in The state error covariance matrix at any given time, in the field of obstacle tracking, It is a 9x9 matrix representing the state error of the obstacle. This represents the prior error covariance matrix, i.e., the matrix of the filter based on... state quantity at time 1 ,predict The state quantity at time t, is obtained Prior state estimation at time 1 hour, The error covariance matrix.
[0018] The process noise covariance matrix represents an increment in system noise during the filter prediction process. It will be predefined based on historical experience.
[0019] In the field of obstacle tracking, the observed measurements of a filter refer to the information about obstacles observed by various sensors, including: their position in three-dimensional space, length, width, height, and orientation angle.
[0020]
[0021] The measurement noise covariance matrix is used to describe... The error. It will be predefined based on the accuracy of different sensors.
[0022] This represents the Kalman gain at time k.
[0023] This represents the observation matrix, which is the transformation matrix used to convert state variables from the state variable space to the observation space. In the field of obstacle tracking, for:
[0024]
[0025] Kalman filters require that the observations input into the filter be time-series. When tracking an obstacle using a single sensor, the time-series nature of the observations input into the filter can be guaranteed. However, when tracking the same obstacle using multiple sensors, the time-series nature of the observations cannot be guaranteed due to differences in exposure time between different sensors, varying processing times of different sensor algorithms, system congestion, and other factors. Therefore, a method is needed to address this issue. Summary of the Invention
[0026] To address the existing technical problems, this invention provides a method for fusing the perception results of multiple sensors for obstacles in rail transit.
[0027] The specific content of this invention is as follows: A method for fusing the perception results of multiple sources of rail transit obstacles, comprising the following steps:
[0028] S1, Establish the vehicle coordinate system;
[0029] S2 receives obstacle observation data output by multiple sensors in the vehicle coordinate system;
[0030] S3 assigns an independent Kalman filter to each obstacle and maintains the state error covariance matrix;
[0031] S4, Time-series observation queue management: For each obstacle, a time-series observation queue is established to dynamically manage multi-source sensor data: observations earlier than the last update time of the Kalman filter are discarded, and new observations are inserted in order of timestamp;
[0032] S5, queue dynamic processing: When the time difference between the first and last ends of the queue is greater than the threshold, the state of the earliest observation input filter in the queue is updated in turn, and the earliest observation is removed from the queue. This operation is repeated until the time difference between the first and last ends of the queue is less than the threshold.
[0033] S6, Obstacle Matching Update: Calculate the Mahalanobis distance between the predicted state and the new observation, and realize cross-sensor obstacle association through the nearest neighbor algorithm;
[0034] S7, Periodic Output: At each predefined interval, the filter for each obstacle is copied; all observations in the time queue are input into the copied filter in chronological order for state updates.
[0035] Furthermore, in S1, the vehicle coordinate system takes the center point of the front wheel axle of the driver's cab in the direction of travel as its origin and the direction of travel as its coordinate system. Positive axis direction, right side is Positive axis direction, directly above is Positive direction of the axis.
[0036] Furthermore, in S3, an independent Kalman filter is assigned to each obstacle, whose state variables include: the obstacle's position coordinates (x, y, z) in the vehicle coordinate system, its size, and its orientation angle. Velocity component ( , ).
[0037] Furthermore, S4 includes: when an obstacle is detected for the first time, initializing the filter state variables using the observations from the first sensor, and... , Initialize to 0, initialize the corresponding dimension of the sensor's state quantity error covariance matrix with the error covariance matrix of the sensor's observations, and set the noise of the velocity dimension in the sensor's state quantity error covariance matrix to a predefined value. Meanwhile, the sensor exposure time is recorded as the last filter update time;
[0038] When any subsequent sensor detects the obstacle, if the sensor's observation time is earlier than the filter's last update time, the observation is discarded; otherwise, the observation is inserted into the time-series observation queue in timestamp order.
[0039] Furthermore, S5 includes: when the time difference between the earliest and latest observations in the queue exceeds a predefined threshold. At that time, remove the observation at the front of the queue and input it into the filter:
[0040] Perform Kalman filter prediction: based on historical states To estimate the prior state of the sensor exposure timestamp, i.e., the predicted value. State error covariance matrix based on historical states Calculate the prior error covariance matrix of the sensor exposure timestamps. ;
[0041] Based on the measurement noise covariance matrix Calculate the Kalman gain of the sensor exposure timestamp. ;
[0042] Kalman gain based on sensor exposure timestamps Prior state estimation of sensor exposure timestamps Observations at the sensor exposure time Calculate the state variables of the sensor at the exposure time. ;
[0043] Based on the Kalman gain at the sensor exposure time and the prior error covariance matrix of the sensor exposure timestamps. Calculate the state error covariance matrix at the sensor exposure time. ;
[0044] Repeat execution until the queue time difference is less than the threshold. .
[0045] Furthermore, S6 includes: when new sensor data is input, performing Kalman prediction on the recorded obstacles using the timestamp of the new sensor data input to obtain a prior state estimate at the moment of sensor exposure. and prior error covariance matrix ; Calculate the Mahalanobis distance between the prior state estimates of all recorded obstacles and the newly observed obstacle observations, and establish the association using the nearest neighbor algorithm; Unmatched newly observed obstacles are initialized as new obstacles according to S4; Newly observed obstacles that are successfully associated are discarded or inserted into the time sequence queue according to S4.
[0046] Furthermore, the formula for calculating the Mahalanobis distance is:
[0047]
[0048] in, This represents the set of prior state estimates for all recorded obstacles at the moment of sensor exposure. This represents the prior state estimate of any recorded obstacle at the moment of sensor exposure. This represents its prior error covariance matrix; The state observation matrix, Upcoming state quantity Transition from the state space to the observation space; This represents the set of all newly observed obstacle measurements by the sensor. This represents the observation of any one of the recorded obstacles. This represents its measurement noise covariance matrix.
[0049] Furthermore, S7 includes: a predefined period per interval. Copy the filter for each obstacle;
[0050] All observations in the time-series queue are input into the replicated filter in chronological order for state updates. Specifically, for each observation:
[0051] Perform Kalman filter prediction: based on historical states To estimate the prior state of the sensor exposure timestamp, i.e., the predicted value. State error covariance matrix based on historical states Calculate the prior error covariance matrix of the sensor exposure timestamps. ;
[0052] Based on the measurement noise covariance matrix Calculate the Kalman gain of the sensor exposure timestamp. ;
[0053] Kalman gain based on sensor exposure timestamps Prior state estimation of sensor exposure timestamps Observations at the sensor exposure time Calculate the state variables of the sensor at the exposure time. ;
[0054] Based on the Kalman gain at the sensor exposure time and the prior error covariance matrix of the sensor exposure timestamps. Calculate the state error covariance matrix at the sensor exposure time. ;
[0055] Repeat the above operation until all observations in the time series queue are entered into the copied filter in chronological order;
[0056] The updated state quantity for each recorded obstacle. The data is collected, organized, and output to the train emergency braking module.
[0057] Furthermore, it also includes S8, lost obstacle removal: if a recorded obstacle exceeds the lost threshold... If no new sensor inputs are established for an obstacle, it is considered that the obstacle has left the field of view and will be marked as lost. Lost obstacles will not be processed and output to the train emergency braking module as described in step seven; for lost obstacles, the deletion threshold will be exceeded again. If no new obstacle is associated with any new sensor input, then the obstacle is removed from the list of recorded obstacles.
[0058] This invention dynamically manages historical observation data through asynchronous data fusion and obstacle matching optimization mechanisms. It eliminates the impact of multi-sensor temporal differences on the algorithm through a time-series queue, and ensures output stability through a replication filter mechanism, avoiding the impact of real-time filtering fluctuations on braking decisions. By predefining the measurement noise covariance matrix corresponding to different sensors, it can flexibly adapt to sensors with different sensing accuracies. It solves the problem of obstacle state estimation error caused by asynchronous multi-sensor temporal data. Attached Figure Description
[0059] The invention will be further explained below with reference to the accompanying drawings.
[0060] Figure 1 This is an external architecture diagram of the multi-source sensor fusion algorithm of the present invention;
[0061] Figure 2 A schematic diagram of initializing an obstacle's Kalman filter using sensor data;
[0062] Figure 3 This is a schematic diagram illustrating whether to insert data into the timing queue based on the sensor observation timestamp and the filter's last update timestamp.
[0063] Figure 4 This is a schematic diagram of the matching and updating module of the present invention;
[0064] Figure 5 This is a schematic diagram of the queue dynamic processing of the present invention;
[0065] Figure 6 This is a schematic diagram of obstacle output. Detailed Implementation
[0066] This invention discloses a method for fusing perception results from multiple sources of sensors for obstructing rail transit obstacles, comprising the following steps:
[0067] Step 1: Establish a system with the center point of the front wheel axle of the driver's cab as the origin and the direction of travel as the reference point. Positive axis direction, right side is Positive axis direction, directly above is The vehicle coordinate system in the positive axis direction.
[0068] Step 2: Receive obstacle observation data output by multiple sensors in the vehicle coordinate system, including visual sensors, lidar, or millimeter-wave radar.
[0069] Step 3: Assign an independent Kalman filter to each obstacle. The state variables of the filter include: the obstacle's position coordinates (x, y, z) in the vehicle's coordinate system, its dimensions (length l, width w, height h), and its orientation angle. velocity components ( , ), and maintain the state error covariance matrix.
[0070] Step 4, Time Series Observation Queue Management:
[0071] When an obstacle is detected for the first time, the filter state variables are initialized using the observations from the first sensor. , Initialize to 0, initialize the corresponding dimension of the sensor's state quantity error covariance matrix with the error covariance matrix of the sensor's observations, and set the noise of the velocity dimension in the error covariance matrix of the state quantity of the bed dryer to a predefined value. Simultaneously, the sensor exposure time is recorded as the last filter update time.
[0072] When any subsequent sensor detects the obstacle: if the sensor's observation time is earlier than the filter's last update time, the observation is discarded; otherwise, the observation is inserted into the time-series observation queue in timestamp order.
[0073] Step 5, Dynamic processing of the queue:
[0074] After completing step 4b and inserting the observations into the time-series observation queue according to their timestamps, this step is executed; when the time difference between the earliest and latest observations in the queue exceeds a predefined threshold. At that time, remove the observation at the front of the queue and input it into the filter:
[0075] Perform Kalman filter predictions (Equations 1 and 2): based on historical states To estimate the prior state of the sensor exposure timestamp, i.e., the predicted value. State error covariance matrix based on historical states Calculate the prior error covariance matrix of the sensor exposure timestamps. ;
[0076] Based on the measurement noise covariance matrix (Based on the sensor type predefined) Calculate the Kalman gain of the sensor exposure timestamp. (Formula 3);
[0077] Kalman gain based on sensor exposure timestamps Prior state estimation of sensor exposure timestamps Observations at the sensor exposure time Calculate the state variables of the sensor at the exposure time. (Formula 4);
[0078] Based on the Kalman gain at the sensor exposure time and the prior error covariance matrix of the sensor exposure timestamps. Calculate the state error covariance matrix at the sensor exposure time. (Formula 5);
[0079] Repeat execution until the queue time difference is less than the threshold. .
[0080] Step 6, Obstacle Matching Update:
[0081] When new sensor data is input, Kalman prediction (Equations 1 and 2) is performed on the recorded obstacles using the timestamp of the new sensor data input to obtain a priori state estimate at the moment of sensor exposure. and prior error covariance matrix ;
[0082] Calculate the Mahalanobis distance between the prior state estimates of all recorded obstacles and the newly observed obstacle observations, and establish the association using the nearest neighbor algorithm;
[0083] For newly observed obstacles that are not matched, initialize them as new obstacles according to step four;
[0084] For newly observed obstacles that have been successfully linked, proceed to step four to determine whether to discard or insert them into the time-series queue;
[0085] Step 7, Periodic Output:
[0086] Each predefined period Copy the filter for each obstacle;
[0087] All observations in the time-series queue are input into the replicated filter in chronological order for state updates. Specifically, for each observation:
[0088] Perform Kalman filter predictions (Equations 1 and 2): based on historical states To estimate the prior state of the sensor exposure timestamp, i.e., the predicted value. State error covariance matrix based on historical states Calculate the prior error covariance matrix of the sensor exposure timestamps. ;
[0089] Based on the measurement noise covariance matrix (Based on the sensor type predefined) Calculate the Kalman gain of the sensor exposure timestamp. (Formula 3);
[0090] Kalman gain based on sensor exposure timestamps Prior state estimation of sensor exposure timestamps Observations at the sensor exposure time Calculate the state variables of the sensor at the exposure time. (Formula 4);
[0091] Based on the Kalman gain at the sensor exposure time and the prior error covariance matrix of the sensor exposure timestamps. Calculate the state error covariance matrix at the sensor exposure time. (Formula 5);
[0092] Repeat the above operation until all observations in the time series queue are entered into the copied filter in chronological order;
[0093] The updated state quantity for each recorded obstacle. The data is collected, organized, and output to the train emergency braking module.
[0094] Step 8, Remove Lost Obstacles:
[0095] In step six, when a recorded obstacle is successfully associated with a new obstacle input by the sensor, it will be discarded or inserted into the timing queue as determined in step four; if a recorded obstacle exceeds the loss threshold... If no new sensor input is established for the obstacle, it is considered that the obstacle has left the field of view and will be marked as lost. Obstacles in the lost state will not be sorted out and output to the train emergency braking module as described in step seven.
[0096] For obstacles that are missing, the deletion threshold is exceeded again. If no new obstacle is associated with any new sensor input, then the obstacle is removed from the list of recorded obstacles.
[0097] In this preferred embodiment, a predefined threshold is used. The output cycle is 1 second. Set to 100ms; initialize the predefined noise values for the velocity dimension in the state variable error covariance matrix. It is 25.
[0098] The noise predefined value setting for the velocity dimension in the initial state quantity error covariance matrix is based on the range of movement speed fluctuations of obstacles (pedestrians, etc.) within the scene.
[0099] The criteria for setting the observation noise covariance matrix in step five include: sensor type, historical data on detection accuracy, and environmental interference factors.
[0100] The criteria for setting the observation noise covariance matrix in step six include: sensor type, historical data on detection accuracy, and environmental interference factors;
[0101] The formula for calculating the Mahalanobis distance in step six is:
[0102]
[0103] in, This represents the set of prior state estimates for all recorded obstacles at the moment of sensor exposure. This represents the prior state estimate of any recorded obstacle at the moment of sensor exposure. This represents its prior error covariance matrix; The state observation matrix, Upcoming state quantity Transition from the state space to the observation space; This represents the set of all newly observed obstacle measurements by the sensor. This represents the observation of any one of the recorded obstacles. This represents its measurement noise covariance matrix;
[0104] In step seven, when copying the filter, the historical state data of the original filter is retained, and the copying operation is executed asynchronously with the real-time filter update operation.
[0105] Loss threshold in step eight The deletion threshold is 1 second. It lasts for 3 seconds.
[0106] The following examples will further illustrate this application.
[0107] Figure 1 This is an external architecture diagram of the multi-source sensor fusion algorithm provided in this application; the train's sensor 101 can be a visual sensor, millimeter-wave radar, or lidar; the train-side computing unit 102 calculates the raw data provided by the sensor 101 and outputs the obstacle detection results of the corresponding sensor in the vehicle coordinate system, which are then sent to the multi-source sensor fusion module 103. The multi-source sensor fusion module 103 can accept any number and any type of sensor 101 and computing unit 102.
[0108] Figure 2 This is a schematic diagram illustrating the initialization of the Kalman filter for the obstacle using sensor data when it is first detected in step four. The obstacle's Kalman filter 201 includes: the state variables 202 and the state error covariance matrix 203. The sensor's perception result 204 of the obstacle includes the sensor's observations of the obstacle 205 and the sensor's measurement noise covariance matrix 206. When the obstacle is first detected, an empty time queue 207 and Kalman filter 201 are created, and the state variables 202 of the Kalman filter are initialized using the sensor's observations of the obstacle 205, and the state error covariance matrix 203 of the Kalman filter is initialized using the sensor's measurement noise covariance matrix 206.
[0109] Figure 3 This is a schematic diagram illustrating step four, where, when a recorded obstacle is observed again by any sensor, the decision to insert it into the timing queue is based on the sensor's observation timestamp and the filter's last update timestamp. If the sensor's observation timestamp is earlier than the filter's last update timestamp, the observation is discarded; otherwise, it is inserted into the timing queue 207 in the order of the sensor's observation timestamps. Observations in the timing queue are always sequential.
[0110] Figure 4This is a schematic diagram of the matching update module in step six. The matching module aims to establish a correlation between the Kalman filter 201 and the perception result 204. The prediction module 401 predicts the sensor observation timestamps to the Kalman filters of all recorded obstacles, obtaining a set 402 of prior state estimates and prior error covariances; it structures the perception results of each obstacle newly observed by the sensor into a set 403 of observations and observation covariance matrices; the observation covariance matrix of the same sensor is the same, as described in the background art, this value is predefined; 403 and 402 are input into the module 404 for calculating Mahalanobis distance, which sequentially calculates the Mahalanobis distance between the prior state estimates of all recorded obstacles and the newly observed obstacle observations, and then through the nearest neighbor matching module 405, for newly observed recorded obstacles that are successfully associated, according to step four, i.e. Figure 3 The process involves determining whether to insert an object into the timing queue based on the sensor observation timestamp and the filter's last update timestamp; for newly observed obstacles that are not matched, the process is followed in step four to initialize and record them.
[0111] Figure 5 This is a schematic diagram of the dynamic queue processing in step five. Each time a new observation is inserted into the time-series queue in step four, it is determined whether the time difference between the earliest and latest observations in the queue exceeds a predefined threshold. If the time difference exceeds the threshold, the earliest observation in the queue is input into the filter, the filter's last update time is modified to the time of that observation, and the observation is removed from the queue. This process is repeated until the time difference between the earliest and latest observations in the queue is less than a predefined threshold. .
[0112] Figure 6 This is a schematic diagram of the obstacle output for step seven. It is generated every predefined period. The filter 201 for each obstacle is copied, resulting in a copied filter 601. The last update time of the copied filter 601 is consistent with that of the original obstacle filter 201; the state variables 602 and state error covariance matrix 603 of the copied filter are consistent with those of the original obstacle filter 202 and state error covariance matrix 203, respectively. The observations in the time-series queue are sequentially input into the copied filter according to the method described in step seven. The state variables of the input filters are then collected, organized, and output to the train emergency braking module.
[0113] This invention employs a dynamic temporal queue mechanism, setting time window thresholds for control and automatically discarding observations earlier than the update timestamp. It dynamically manages historical observation data, resolving the conflict between the Kalman filter and non-temporally sequential observations caused by asynchronous multi-sensor timing. A two-layer filter architecture is used: the main filter processes queued observations in real time, while the replica filter outputs the stable state. The main and replica filters are decoupled, ensuring the temporal order of observations input to both filters. A matching-update decoupling design is employed, using a nearest neighbor algorithm based on predicted state matching. Observational data is asynchronously inserted into the queue, achieving a balance between sensor data throughput and system real-time performance. This invention is applicable to multi-source sensing systems such as train vision, lidar, and millimeter-wave radar.
[0114] Many specific details have been set forth in the foregoing description to provide a thorough understanding of the present invention. However, the above description is merely a preferred embodiment of the present invention, and the present invention can be implemented in many other ways different from those described herein. Therefore, the present invention is not limited to the specific embodiments disclosed above. Furthermore, any person skilled in the art can make many possible variations and modifications to the technical solutions of the present invention, or modify them into equivalent embodiments, using the methods and techniques disclosed above, without departing from the scope of the present invention. Any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention, without departing from the content of the present invention, shall still fall within the protection scope of the present invention.
Claims
1. A method for fusing multi-source sensor perception results of obstacles in rail transit, characterized in that: Includes the following steps: S1, Establish the vehicle coordinate system; S2 receives obstacle observation data output by multiple sensors in the vehicle coordinate system; S3 assigns an independent Kalman filter to each obstacle and maintains the state error covariance matrix; S4, Time-series observation queue management: For each obstacle, a time-series observation queue is established to dynamically manage multi-source sensor data: observations earlier than the last update time of the Kalman filter are discarded, and new observations are inserted in order of timestamp; S5, queue dynamic processing: When the time difference between the first and last ends of the queue is greater than the threshold, the state of the earliest observation input filter in the queue is updated in turn, and the earliest observation is removed from the queue. This operation is repeated until the time difference between the first and last ends of the queue is less than the threshold. S6, Obstacle Matching Update: Calculate the Mahalanobis distance between the predicted state and the new observation, and realize cross-sensor obstacle association through the nearest neighbor algorithm; S7, Periodic Output: At each predefined interval, the filter for each obstacle is copied; all observations in the time queue are input into the copied filter in chronological order for state updates.
2. The method for fusing multi-source sensor perception results of rail transit obstacles according to claim 1, characterized in that: In S1, the vehicle coordinate system takes the center point of the front wheel axle of the driver's cab in the direction of travel as its origin and the direction of travel as its coordinate system. Positive axis direction, right side is Positive axis direction, directly above is Positive direction of the axis.
3. The method for fusing multi-source sensor perception results of rail transit obstacles according to claim 2, characterized in that: In S3, an independent Kalman filter is assigned to each obstacle, whose state variables include: the obstacle's position coordinates (x, y, z) in the vehicle coordinate system, its size, and its orientation angle. Velocity component ( , ).
4. The method for fusing multi-source sensor perception results of rail transit obstacles according to claim 2, characterized in that: S4 includes: When an obstacle is detected for the first time, the filter state variables are initialized using the observations from the first sensor. , Initialize to 0, initialize the corresponding dimension of the sensor's state quantity error covariance matrix with the error covariance matrix of the sensor's observations, and set the noise of the velocity dimension in the sensor's state quantity error covariance matrix to a predefined value. Meanwhile, the sensor exposure time is recorded as the last filter update time; When any subsequent sensor detects the obstacle, if the sensor's observation time is earlier than the filter's last update time, the observation is discarded; otherwise, the observation is inserted into the time-series observation queue in timestamp order.
5. The method for fusing multi-source sensor perception results of rail transit obstacles according to claim 1, characterized in that: S5 includes: When the time difference between the earliest and latest observations in the queue exceeds a predefined threshold At that time, remove the observation at the front of the queue and input it into the filter: Perform Kalman filter prediction: based on historical states To estimate the prior state of the sensor exposure timestamp, i.e., the predicted value. ; State error covariance matrix based on historical states Calculate the prior error covariance matrix of the sensor exposure timestamps. ; Based on the measurement noise covariance matrix Calculate the Kalman gain of the sensor exposure timestamp. ; Kalman gain based on sensor exposure timestamps Prior state estimation of sensor exposure timestamps Observations at the sensor exposure time Calculate the state variables of the sensor at the exposure time. ; Based on the Kalman gain at the sensor exposure time and the prior error covariance matrix of the sensor exposure timestamps. Calculate the state error covariance matrix at the sensor exposure time. ; Repeat execution until the queue time difference is less than the threshold. .
6. The method for fusing multi-source sensor perception results of rail transit obstacles according to claim 1, characterized in that: S6 includes: When new sensor data is input, Kalman prediction is performed on the recorded obstacles using the timestamp of the new sensor data input to obtain a priori state estimate at the moment of sensor exposure. and prior error covariance matrix ; Calculate the Mahalanobis distance between the prior state estimates of all recorded obstacles and the newly observed obstacle observations, and establish the association using the nearest neighbor algorithm; Unmatched newly observed obstacles are initialized as new obstacles according to S4; For newly observed obstacles that are successfully associated, determine whether to discard or insert them into the timing queue according to S4.
7. The method for fusing multi-source sensor perception results of rail transit obstacles according to claim 1, characterized in that: The formula for calculating Mahalanobis distance is: ， in, This represents the set of prior state estimates for all recorded obstacles at the moment of sensor exposure. This represents the prior state estimate of any recorded obstacle at the moment of sensor exposure. This represents its prior error covariance matrix; The state observation matrix, Upcoming state quantity Transition from the state space to the observation space; This represents the set of all newly observed obstacle measurements by the sensor. This represents the observation of any one of the recorded obstacles. This represents its measurement noise covariance matrix.
8. The method for fusing multi-source sensor perception results of rail transit obstacles according to claim 1, characterized in that: S7 includes: Each predefined period Copy the filter for each obstacle; All observations in the time-series queue are input into the replicated filter in chronological order for state updates. Specifically, for each observation: Perform Kalman filter prediction: based on historical states To estimate the prior state of the sensor exposure timestamp, i.e., the predicted value. ; State error covariance matrix based on historical states Calculate the prior error covariance matrix of the sensor exposure timestamps. ; Based on the measurement noise covariance matrix Calculate the Kalman gain of the sensor exposure timestamp. ; Kalman gain based on sensor exposure timestamps Prior state estimation of sensor exposure timestamps Observations at the sensor exposure time Calculate the state variables of the sensor at the exposure time. ; Based on the Kalman gain at the sensor exposure time and the prior error covariance matrix of the sensor exposure timestamps. Calculate the state error covariance matrix at the sensor exposure time. ; Repeat the above operation until all observations in the time series queue are entered into the copied filter in chronological order; The updated state quantity for each recorded obstacle. The data is collected, organized, and output to the train emergency braking module.
9. The method for fusing multi-source sensor perception results of rail transit obstacles according to claim 1, characterized in that: It also includes S8, Lost Obstacle Removal: If a recorded obstacle exceeds the loss threshold... If no new sensor inputs are established for an obstacle, it is considered that the obstacle has left the field of view and will be marked as lost. Lost obstacles will not be processed and output to the train emergency braking module as described in step seven; for lost obstacles, the deletion threshold will be exceeded again. If no new obstacle is associated with any new sensor input, then the obstacle is removed from the list of recorded obstacles.