Multi-sensor data fusion method based on adaptive Kalman filtering

Abstract

The invention relates to the technical field of intelligent transportation, and particularly discloses a multi-sensor data fusion method based on adaptive Kalman filtering, which is suitable for vehicle trajectory tracking in a tunnel scene. The method comprises the steps of sensor deployment and data acquisition, data preprocessing and anomaly detection, multi-target data association, adaptive filtering fusion and state estimation. By introducing an anomaly detection mechanism and dynamically adjusting a filtering estimation error covariance, the influence of abnormal data such as a multipath effect and electromagnetic interference is effectively suppressed, and the tracking precision and stability of the position, speed and trajectory of the vehicle are improved. The method is remarkably superior to a traditional filtering algorithm in a complex tunnel environment, has the technical advantages of high-precision tracking, high robustness, high adaptability, high real-time performance, comprehensive functions and the like, and can remarkably improve the reliability and performance of a tunnel vehicle monitoring system; the method is suitable for multiple key fields of tunnel safety early warning, accident prevention, traffic flow optimization, emergency response and the like.

Description

Technical Field

[0001] The present invention relates to the field of intelligent transportation. Specifically, it relates to a multi-sensor data fusion method based on adaptive Kalman filtering, which is applicable to vehicle trajectory tracking in tunnel scenarios. Background Art

[0002] With the acceleration of urbanization, tunnels, as a key part of urban roads and transportation arteries, have seen a year-on-year increase in traffic volume and a continuous improvement in traffic complexity. However, the special environment inside tunnels, such as poor lighting conditions, complex airflows, and the impact of sudden accidents and obstacles, has brought many challenges to vehicle monitoring. Currently, tunnel vehicle monitoring mainly relies on single-sensor devices, such as cameras, millimeter-wave radars, and geomagnetic sensors. However, these traditional methods all have significant limitations and are difficult to meet the growing monitoring requirements for high precision, real-time performance, and robustness.

[0003] Limitations of camera technology: Although cameras can provide rich visual information, the image quality deteriorates severely in low-light, backlight, and dusty environments in tunnels, resulting in difficulties in vehicle recognition and trajectory tracking. At the same time, the field of view of cameras is limited, unable to fully cover the entire tunnel space, prone to monitoring blind spots, and the processing effect for complex traffic behaviors such as vehicle occlusion and lane change is not good, making it difficult to stably extract vehicle motion information.

[0004] Deficiencies of millimeter-wave radar technology: Although millimeter-wave radars have advantages such as high ranging and velocity measurement accuracy and all-weather operation, in tunnel environments, due to multipath effects and echo interference caused by walls, obstacles, etc., the measured data shows anomalies and instability. In addition, the detection accuracy of millimeter-wave radars for lateral targets decreases with increasing distance, and it is easily interfered by adjacent vehicles, resulting in false detections and missed detections, making it difficult to be used alone as a reliable basis for vehicle tracking.

[0005] Disadvantages of geomagnetic sensor technology: Geomagnetic sensors are low-cost and easy to deploy, and judge vehicle position and speed by detecting magnetic field disturbances caused by vehicles. However, in such a complex electromagnetic environment as a tunnel, various electrical equipment, vehicle electronic equipment, and metal structures such as steel bars seriously interfere with the geomagnetic signal, resulting in a large amount of noise and outliers in the original data collected by the geomagnetic sensor, reducing its positioning accuracy and reliability. In addition, the spatial resolution of geomagnetic sensors is limited, and the ability to distinguish multi-lane and high-density traffic flows is insufficient, prone to problems such as trajectory confusion and incorrect association.

[0006] In order to overcome the limitations of the above-mentioned single sensor technology and improve the accuracy and stability of tunnel vehicle monitoring, multi-sensor data fusion has become an effective solution. Although some existing multi-sensor fusion methods, such as weighted average method and Kalman filter method, can integrate the advantages of different sensors to a certain extent, they still face many difficulties in actual tunnel scene applications.

[0007] The defects of weighted average fusion: In the uncertain dynamic environment of tunnels, the quality and reliability of different sensor data will change, while the weights of the weighted average method are generally fixed and cannot be dynamically adjusted according to the real-time data quality, resulting in the fusion results being easily affected by low-quality sensor data, reducing the system's adaptability and anti-interference ability. For abnormal driving behavior of vehicles and sudden traffic incidents in the tunnel, it is impossible to reflect the vehicle status in a timely and accurate manner.

[0008] The shortcomings of traditional Kalman filter fusion: Kalman filtering is based on accurate assumptions about the system model and accurate estimates of the statistical characteristics of noise. However, in complex and changeable scenarios such as tunnels, the vehicle motion model is often nonlinear and inaccurate, and the statistical characteristics of process noise and measurement noise are not easy to obtain. This makes it difficult to guarantee the filtering performance of traditional Kalman filtering, and it is prone to problems such as estimation bias and filtering divergence. Especially in complex motion states such as vehicle lane changes, acceleration, and braking, the filter cannot track vehicle state changes in real time, resulting in reduced trajectory tracking accuracy. At the same time, most existing fusion systems are limited to specific tunnel environments and sensor configurations, and have poor adaptability to different tunnel structures, sensor layouts, and traffic characteristics, making it difficult to achieve widespread promotion and application.

[0009] Therefore, how to design a high-precision, robust, and adaptable multi-sensor data fusion vehicle tracking method based on the characteristics of tunnel scenes is one of the key issues that need to be urgently solved in the field of intelligent transportation.

[0010] The invention patent "Data Fusion Method and System Based on Multi-Sensors" (Publication No.: CN119004059A) discloses a data fusion method and system based on multi-sensors. By combining the horizontal comparison between different sensors and the longitudinal analysis of the same sensor over time, the accuracy of data fusion and the robustness of the system are significantly improved. The horizontal comparison captures the differences between sensors, and the longitudinal analysis evaluates the performance changes of sensors over time, providing a comprehensive assessment of the health status of sensors. At the same time, by setting the first evaluation coefficient and the second evaluation coefficient to evaluate the risks of the current state and the future state respectively, high-risk sensors can be identified in a timely manner and potential problems can be foreseen. Moreover, the adaptive setting of the observation time and the observation window reduces the consumption of computing resources, improves the response speed of the system, and meets diverse application requirements. However, before data fusion, it is necessary to perform preprocessing operations such as partitioning and normalizing the data of different sensors. This requires the data to have a certain quality and format, and has relatively high requirements for the integrity and consistency of the data. If there are problems such as missing data, outliers, or inconsistent formats in the sensor data, it may affect the effect of preprocessing and further affect the entire data fusion process.

[0011] The invention patent "A Multi-Sensor Data Fusion Method" (Publication No.: CN114202025A) discloses a multi-sensor data fusion method. By performing coarse clustering and fine clustering to remove dirty data and associate data with higher credibility, then performing spatio-temporal alignment on the target data within the same cluster, and making an identity determination on the spatio-temporally aligned target data, the target data that does not meet the identity is deleted from the cluster, and finally the target data that passes the identity determination in the cluster is fused. The data fusion of multiple targets of multiple sensors is realized, the computational amount of spatio-temporal alignment and identity determination is reduced, the accuracy of data fusion is effectively improved, and large errors are avoided. However, the identity determination adopted by this method depends on the comparison between the weighted average Euclidean distance and the preset distance threshold. This determination method may not be able to fully consider the dynamic characteristics of the target data and environmental interference, resulting in misjudgment or missed judgment, and affecting the accuracy of data fusion. Moreover, the performance of this method depends on the quality of the sensor data. If there are missing data, noise, or outliers in the sensor data, it may affect the accuracy of clustering, spatio-temporal alignment, and identity determination, and further affect the final data fusion result.

[0012] In summary, it is difficult for the prior art to achieve high-precision and low-cost dynamic data fusion in tunnel scenarios. Summary of the Invention

[0013] The present invention aims to solve the problems existing in traditional vehicle trajectory tracking technologies in tunnel scenarios, such as insufficient accuracy, poor robustness, and weak adaptability, and proposes a vehicle tracking method based on the data fusion of millimeter-wave radar and geomagnetic sensors using adaptive Kalman filtering. This method can make full use of the complementary advantages of the two sensors (including millimeter-wave radar and geomagnetic sensors), achieve innovative breakthroughs in multiple links such as sensor data preprocessing, association, fusion, and state estimation, so as to realize high-precision, high-robustness, and high-adaptability tracking of vehicles in tunnels. The technical problems to be solved by the present invention are realized through the following technical solutions:

[0014] A multi-sensor data fusion method based on adaptive Kalman filtering for vehicle trajectory tracking in tunnel scenarios, comprising the following steps:

[0015] S1: Deploy sensors and collect original data in the tunnel scenario, including the collaborative deployment of millimeter-wave radar and geomagnetic sensors, collect the position, speed, and acceleration information of the vehicle using the millimeter-wave radar, monitor the magnetic field change data generated when the vehicle passes using the geomagnetic sensor, and construct a state equation and a measurement equation;

[0016] S2: Preprocess sensor data and detect anomalies, including coordinate system conversion, time alignment, and outlier detection;

[0017] S3: Associate and cluster multi-sensor data, adopt a multi-target tracking data association method based on the Hungarian algorithm to associate the preprocessed millimeter-wave radar data and geomagnetic sensor data;

[0018] S4: Adaptive Kalman filtering fusion and state estimation, design an adaptive Kalman filtering algorithm based on anomaly detection, quantify the anomaly of the measurement by calculating the ratio of the innovation covariance to the actual covariance, and introduce an adaptability factor to adjust the estimation error covariance of the filter in real time to obtain the estimated values of the precise position and speed of the vehicle.

[0019] In an embodiment of the present invention, the S1 includes: deploy a millimeter-wave radar every 150m on the tunnel wall on one side of the tunnel, deploy a geomagnetic sensor every 15m on the lane lines on both sides of the tunnel, take the midpoint of the lane line of the first geomagnetic sensor on the rightmost lane line in the vehicle driving direction as the coordinate origin, its X-axis coincides with the center line of the rightmost lane line, and the Y-axis points to the leftmost lane direction.

[0020] In one embodiment of the present invention, in the step S2, for the data collected by the millimeter-wave radar, coordinate system conversion is first performed to unify it into the tunnel global coordinate system; the geomagnetic sensor data collected by the geomagnetic sensor and the millimeter-wave radar data collected by the millimeter-wave radar are aligned in time to eliminate the time synchronization error and ensure the consistency of the data in the time dimension; the hypothesis testing method is used to detect outliers in the millimeter-wave radar data and the geomagnetic sensor data, and the abnormal measurement points are identified and removed by calculating the chi-square threshold.

[0021] In one embodiment of the present invention, in the step S3, a data association matrix is constructed, and the considered factors include the proximity of timestamps, the proximity of spatial positions, and the similarity of motion directions; by solving the data association matrix, the measurement data of the same vehicle at different times and on different measurement devices can be correctly matched to the corresponding trajectory clusters, so as to solve the problems of data fragmentation and multi-target interference, where the measurement devices include millimeter-wave radar and geomagnetic sensors.

[0022] In one embodiment of the present invention, in the step S4, the abnormality of the measurement is quantified by calculating the ratio of the innovation covariance to the actual covariance, and an adaptation factor is introduced to adjust the estimation error covariance of the filter in real time. When abnormal measurement data is detected, the adaptation factor can automatically reduce the influence of the abnormal data on the filtering result to prevent the filter from diverging.

[0023] In one embodiment of the present invention, the state equation includes the position and speed state variables of the vehicle, and the measurement equation fuses the measurement data of the millimeter-wave radar and the geomagnetic sensor. In the state prediction stage, the dynamic model of the vehicle is used to predict the vehicle state at the next moment, and the state is updated through adaptive Kalman filtering to obtain the estimated values of the precise position and speed of the vehicle.

[0024] Compared with the traditional technology, the present invention has the following significant advantages:

[0025] 1. High-precision tracking: By integrating the complementary advantages of the millimeter-wave radar and the geomagnetic sensor, and using the adaptive Kalman filtering algorithm to accurately fuse the data, the tracking accuracy of the vehicle position, speed and trajectory is significantly improved, and high-resolution and high-confidence tracking of the vehicle can be achieved in a complex tunnel environment.

[0026] 2. Strong robustness: In view of the complex interference and data uncertainty in the tunnel scenario, the present invention effectively suppresses the influence of sensor noise, multipath effect, electromagnetic interference, etc. through technologies such as sensor data preprocessing, outlier detection, and adaptive filtering, improves the anti-interference ability and stability of the system, and can ensure the continuity and accuracy of vehicle tracking even under harsh environmental conditions.

[0027] 3. High adaptability: The system architecture and algorithm design of the present invention are flexible and can adapt to changes in the structure, size, and traffic flow density of different tunnels. Through the adaptive adjustment of sensor deployment parameters and the online update of the model, the system can quickly respond to changes in the tunnel environment and traffic conditions, achieving effective monitoring and management of various complex scenarios.

[0028] 4. Strong real-time performance: Algorithms such as data processing, association, clustering, and state estimation are all optimized and designed to be able to run quickly in a real-time data stream environment, meeting the requirements of high-speed movement and real-time monitoring of tunnel vehicles, and providing timely support for traffic control and safety warning.

[0029] 5. Comprehensive functions: In addition to vehicle trajectory tracking, the present invention also has functions of trajectory prediction and abnormal behavior recognition, which can provide rich decision-making support information for tunnel operation management, such as traffic flow prediction, accident warning, congestion relief, etc., improving the overall intelligent level of tunnel traffic. Therefore, the method of the present invention has extremely important value for improving the safety, traffic efficiency, and emergency response ability of tunnel traffic, and has broad application prospects and popularization significance. Brief Description of the Drawings

[0030] Figure 1 It is a flowchart of a multi-sensor data fusion method based on adaptive Kalman filter provided by an embodiment of the present invention;

[0031] Figure 2 It is a schematic diagram of multi-sensor deployment in a tunnel scenario provided by an embodiment of the present invention;

[0032] Figure 3 It is a schematic diagram of coordinate transformation provided by an embodiment of the present invention;

[0033] Figure 4 It is a schematic diagram of time alignment provided by an embodiment of the present invention;

[0034] Figure 5 It is a data fusion flowchart based on adaptive Kalman filter in an embodiment of the present invention;

[0035] Figure 6 and Figure 7 It is a comparison experimental test chart of true measurement values and estimated values before and after adding the filtering algorithm. Detailed Embodiment

[0036] In a tunnel scenario, as the detection distance of a millimeter-wave radar increases, the lateral detection accuracy gradually decreases, and multipath interference measurements also occur. When detecting a target, geomagnetic sensors deployed on both sides of the road lane lines are prone to missing detections and false detections for the middle lane (i.e., a large vehicle in the middle lane triggers false detections of the geomagnetic sensors on both sides of the road), resulting in measurement outliers. These measurement outliers mainly refer to inaccurate lateral detections of vehicles at long distances by the millimeter-wave radar, multipath interference measurements, and measurements caused by the middle lane accidentally triggering the geomagnetic sensors. When the Kalman filter updates these measurement outliers, abnormal estimated values will be generated, and even cause the filter to diverge. To achieve vehicle tracking using multi-sensor data fusion in a tunnel scenario, the present invention proposes a multi-sensor data fusion method based on adaptive filtering. This method can identify measurement outliers according to the quality of the measurements and adaptively adjust the Kalman filter, thereby improving the accuracy and reliability of the estimation results. The multi-sensor data fusion method specifically includes the following steps:

[0037] S1: Define the system model, deploy sensors in the tunnel scenario and collect raw data, including the collaborative deployment of millimeter-wave radars and geomagnetic sensors. Use the millimeter-wave radar to collect information on the position, speed, and acceleration of the vehicle, and use the geomagnetic sensor to monitor the magnetic field change data generated when the vehicle passes by.

[0038] Specifically, please refer to Figure 2 , Figure 2 which is a schematic diagram of the deployment of multi-sensors in a tunnel scenario provided by an embodiment of the present invention. Specifically, a millimeter-wave radar is deployed every 150m on the tunnel wall on one side of the tunnel, and a geomagnetic sensor is deployed every 15m on both sides of the lane lines in the tunnel. The rightmost lane line in the vehicle driving direction is marked as lane line 0, and the jth geomagnetic sensor on the ith lane line is marked as geomagnetic sensor b i,j . The midpoint of the lane line of the first geomagnetic sensor on the rightmost lane line in the vehicle driving direction is the coordinate origin, its X-axis coincides with the center line of the rightmost lane line, and the Y-axis points to the leftmost lane direction. Subsequently, use the millimeter-wave radar to collect information on the position, speed, and acceleration of the vehicle, and use the geomagnetic sensor to monitor the magnetic field change data generated when the vehicle passes by.

[0039] For a linear system, it is represented by the discrete-time linear state dynamic equation as follows:

[0040] x k = F k x k-1 + ω k-1 (1)

[0041] The measurement equation is expressed as:

[0042]

[0043] Equations (1) and (2) are the state equation and the measurement equation respectively, where k represents the discrete-time index, and x k = [p x,k , p y,k , v x,k , v y,k T is the state vector at time k, p x,k and p y,k are the x and y direction positions of the vehicle motion at time k respectively, v x,k is the longitudinal speed of the vehicle motion at time k, and v y,k is the lateral speed of the vehicle motion at time k. F k is the state transition matrix of the target, ω k-1 is the process noise at time k - 1, which is generally assumed to be Gaussian white noise with a mean of 0 and a covariance of Q ω . represents the measurement from sensor i at time k, where r represents the millimeter-wave radar and b represents the geomagnetic sensor in i = {r, b}. The measurement vector from the millimeter-wave radar at time k is are the position and speed measurements in the x and y directions of the millimeter-wave radar at time k respectively; the measurement from the geomagnetic sensor at time k is is the position measurement in the x direction of the geomagnetic sensor at time k. is the known measurement matrix. The measurement matrix of the millimeter-wave radar is The measurement matrix of the geomagnetic sensor is is the measurement noise, which is assumed to be Gaussian white noise with a mean of 0 and a covariance of . During the vehicle tracking process, it is assumed that the measurement noises of the millimeter-wave radar and the geomagnetic sensor are independent, and the measurement noise and the process noise are uncorrelated.

[0044] S2: Perform sensor data preprocessing and anomaly detection, including coordinate system conversion, time alignment, and outlier detection.

[0045] In the fusion perception system, the observations of the target by each sensor (this sensor includes the geomagnetic sensor and the millimeter-wave radar) are independent of each other. Their observation results are all based on the local coordinate system of each sensor. Moreover, due to differences in working methods, software and hardware levels, etc., the sampling frequencies and sampling start times of each sensor are also different. If multi-sensor fusion perception is to be achieved, data preprocessing of the sensors must be carried out first, that is, to ensure that the measurement values of each sensor are at the same reference time and in the same coordinate system.

[0046] ​A fusion perception system generally consists of multiple sensors. Each sensor makes independent observations. Therefore, before information fusion, the data of each sensor needs to be converted to a unified coordinate system. Each sensor has completed corresponding detection and tracking in its respective local coordinate system. Now, it is necessary to perform coordinate transformation on the target-level data output by it and convert it to the tunnel global coordinate system. The relative relationship between the local coordinate system and the tunnel global coordinate system can be described by displacement and rotation. For two rectangular coordinate systems with non-coincident coordinate origins, generally, rotation is performed first and then translation to complete the coordinate transformation.

[0047] Specifically, as Figure 3 shown, assume that the coordinates of a point P in the reference coordinate system XOY are (x, y), the origin is O, and it rotates by θ around point O. Then the coordinates (x′, y′) of point P in the new coordinate system X′OY′ are obtained using trigonometric functions:

[0048]

[0049] Among them, when the coordinate system rotates clockwise, θ is positive, and when it rotates counterclockwise, θ is negative.

[0050] Compared with the rotation transformation, the geometric relationship of the translation transformation is simpler. Let the coordinates of the origin O of the reference coordinate system in the translated coordinate system be (a, b). Then the coordinates of point P in the new coordinate system X′OY′ after translation are (x + a, y + b).

[0051] In summary, the coordinate transformation relationship between the local coordinate system of each sensor and the tunnel global coordinate system can be expressed as:

[0052]

[0053] Among them, (x, y) is the position of the target in the local coordinate system of the sensor, (x′, y′) is the position of the target in the tunnel global coordinate system after coordinate transformation, θ is the orientation of the Y+ axis of each sensor's local coordinate system, and a and b are the positions of each sensor in the tunnel global coordinate system. In addition to the need to perform coordinate transformation on the position information of the target, it is also necessary to perform coordinate transformation on the velocity information of the target. For velocity, only rotation transformation is required.

[0054] Due to differences in factors such as working methods, software and hardware levels, etc. among various sensors, there are differences in their sampling frequencies and starting times of sampling, and time alignment needs to be performed before information fusion. Taking the sensing moment of the millimeter-wave radar as the reference moment for time synchronization, the target state is predicted for the latest sensing result of the geomagnetic sensor before the sensing moment of the millimeter-wave radar to obtain the equivalent measurement value at the reference moment. During the movement of a maneuvering target, the magnitude and direction of its speed are constantly changing, but within a short period (for example, not exceeding one sampling period), it can be regarded as a uniform linear motion state for processing. Therefore, the constant velocity (CV) model is used to predict the state of the sensing result of the geomagnetic sensor.

[0055] After data preprocessing, outlier detection needs to be carried out. There are multipath interferences and constraints on the effective detection distance in the detection of millimeter-wave radar in a tunnel environment, and there will also be adjacent lane interferences in the detection of vehicles by geomagnetic sensors. The measurement quality from roadside millimeter-wave radar and geomagnetic sensors is considered during fusion. To ensure the accuracy and robustness of the Kalman filter, hypothesis testing is used to judge the quality of the measurement, identify and eliminate some outliers in the measurement. Use γ k to represent whether the measurement is normal. Assume that γ k is a random sequence of Bernoulli distribution, with representing the probability that the measurement is credible, representing the probability that the measurement is abnormal. Then, judge whether takes the value of 1, that is, judge whether the measurement is credible, and identify the abnormal measurement. If the measurement i = {r, b} is received at the current estimation moment k, where r represents the millimeter-wave radar and b represents the geomagnetic sensor, then the calculated value of the measurement residual can be expressed as:

[0056]

[0057]

[0058] where, is the estimation error of the prior target state. is a Gaussian distribution with a mean of 0 and a variance of .

[0059]

[0060] Here, define

[0061]

[0062] where, ρ k~χ 2 (n) is a chi-squared distribution with n degrees of freedom, whose mean and variance are n and 2n respectively, where n is the dimension of the measurement state vector. Through hypothesis testing, H0: γ k = 1, H1: γ k = 0, is the one-sided chi-squared χ 2 value with a confidence level of α. Use ρ k to determine whether the measurement is a reliable measurement. If then the measurement is an unreliable measurement or a faulty measurement, and the event H1: γ k = 0 holds. If the measurement is a reliable measurement, the event H0: γ k = 1 holds and can be used to update the state of the target.

[0063] S3: Multi-sensor data association and clustering. Adopt a multi-target tracking data association method based on the Hungarian algorithm to associate the preprocessed millimeter-wave radar data and geomagnetic sensor data.

[0064] The basic algorithm of multi-sensor data association is to use the idea of moving back the millimeter-wave radar target position (similar to the time window), calculate the difference between the x-axis of the current geomagnetic sensor measurement and the x-axis of the millimeter-wave radar target after moving back, and the difference in the y-axis position to form a cost matrix, and then use the Hungarian algorithm to perform association matching between the targets generated by the millimeter-wave radar and the geomagnetic sensor measurements.

[0065] Specifically, as Figure 4 shown, first traverse all current millimeter-wave radar targets and perform motion compensation, that is, align the millimeter-wave radar targets to the time stamp of the geomagnetic sensor measurement. Because the geomagnetic sensor has a large time delay, it is necessary to calculate the time difference Δt between the geomagnetic sensor measurement and the current time of the millimeter-wave radar target here (time difference Δt = time stamp of the geomagnetic sensor measurement - time stamp of the millimeter-wave radar target). Then, calculate the x-axis and y-axis positions of the millimeter-wave radar target and the geomagnetic sensor measurement respectively.

[0066] For the x-axis and y-axis positions of the millimeter-wave radar target: First, use the uniform motion formula and the current x-axis position x and velocity v x of the millimeter-wave radar target to calculate the x-axis position before |Δt|, denoted as x r :

[0067] x r = x + v x Δt (8)

[0068] Its y-axis position uses the y-axis value of the center line of the lane where the millimeter-wave radar target was located at the previous moment as its y-axis position, denoted as y. r For the measurement value of the geomagnetic sensor: its x-axis position is the distance measured by this geomagnetic sensor in the global coordinate system of the tunnel, denoted as x. b Its y-axis position is the y-axis value of the center line corresponding to the lane line where the detected geomagnetic sensor is located, denoted as y. b .

[0069] Next, calculate the distance differences in the x-axis and y-axis between the regressed millimeter-wave radar target and the measurement value of the geomagnetic sensor, denoted as Δx and Δy, and calculate the cost matrix cost.

[0070] Δx = x r - x b (9)

[0071] Δy = y r - y b (10)

[0072]

[0073] After the traversal is completed, the cost matrix cost formed by using the measurement values of n geomagnetic sensors and m radar targets is obtained, and the Hungarian algorithm is used for one-to-one association. The smaller the cost value, the greater the possibility of association.

[0074] S4: Adaptive Kalman filter fusion and state estimation. Design an adaptive Kalman filter algorithm based on outlier detection. Quantify the outlier degree of the measurement by calculating the ratio of the innovation covariance to the actual covariance, and introduce an adaptation factor to adjust the estimation error covariance of the filter in real time to obtain the estimated values of the vehicle's precise position and speed.

[0075] As Figure 5 shown, after data association, in view of the uncertainty of sensor data and the dynamic changes of the vehicle motion state in the tunnel environment, an adaptive Kalman filter algorithm based on outlier detection is designed. This algorithm quantifies the outlier degree of the measurement by calculating the ratio of the innovation covariance to the actual covariance, and introduces an adaptation factor to adjust the estimation error covariance of the filter in real time. When abnormal measurement data is detected, the adaptation factor can automatically reduce the influence of the abnormal data on the filtering result, prevent the filter from diverging, and improve the robustness of the filter. Based on the tunnel vehicle motion model, the state equation includes state variables such as the vehicle's position and speed, and the measurement equation fuses the measurement data of the millimeter-wave radar and the geomagnetic sensor. In the state prediction stage, use the vehicle's dynamic model to predict the vehicle state at the next moment, and perform state update through adaptive Kalman filtering to obtain the estimated values of the vehicle's precise position, speed, etc.

[0076] Specifically, according to the above, the calculated innovation covariance can be obtained as follows:

[0077]

[0078] The actual innovation covariance is as follows:

[0079]

[0080] The calculated innovation covariance is obtained and the actual innovation covariance After that, in order to quantitatively analyze the quality of the measurement, the concept of DoA is introduced. DoA (Degree of Abnormality) is an index used to quantify the degree of measurement abnormality, which is obtained by the ratio of the actual innovation covariance to the calculated innovation covariance. The actual innovation covariance is obtained from the actual measurement data, while the calculated innovation covariance is obtained by Kalman filter estimation. This index can be used to estimate the degree of motion uncertainty or external abnormal measurement uncertainty in real time. If the ratio of the actual innovation covariance to the calculated innovation covariance is close to 1, it means that the noise statistical characteristics predicted by the filter are relatively consistent with the actual noise. On the contrary, if this ratio is far from 1, that is, the actual innovation covariance significantly deviates from the calculated innovation covariance, it indicates that there is a large difference between the noise level in the actual observed data and the uncertainty predicted by the filter, that is, the degree of measurement abnormality is large. At this time, it is necessary to adaptively adjust the Kalman filter to prevent abnormal estimated values from being generated when updating these abnormal measurement values in the Kalman filter, resulting in filter divergence.

[0081] The specific calculation formula of the measurement abnormality index is as follows:

[0082]

[0083] where n is the dimension of the measurement vector, and tr is the trace operation. Using the idea of a sliding window, save a total of L (this sliding window time length needs to be set by trying according to the actual application scenario) D k data, calculate the k mathematical expectation of D

[0084]

[0085] An adaptation factor α is established according to the change of the measurement abnormality index, then there is

[0086]

[0087] State prediction (state update):

[0088]

[0089] P k|k-1 = F k|k-1 P k-1 F k|k-1 T + Q k-1 (18)

[0090] wherein, is the target state estimate predicted from time k-1 to time k, and F k|k-1 is the state transition matrix from time k-1 to time k, is the target state estimate at time k-1, and P k|k-1 is the target state estimation error covariance matrix predicted from time k-1 to time k, and P k-1 is the target state estimation error covariance matrix at time k-1, and Q k-1 is the process noise covariance matrix at time k-1.

[0091] Filter correction (measurement update):

[0092]

[0093]

[0094]

[0095] wherein, is the posterior estimate of the filter update, is the state estimate predicted from time k-1 to time k, and K k is the Kalman gain of the filter update, is the measurement residual, and P k|k-1 is the state estimation error covariance matrix predicted from time k-1 to time k, and P k is the posterior estimate error covariance matrix corrected by the filter update, αK k H i k P k|k-1 is the compensation term of the estimation error covariance matrix. α is the adaptation factor. Increasing the P k matrix can make the Kalman filter adjust the state estimate more cautiously and track the change of the system state more conservatively. It is beneficial in the case of fast system dynamic changes and can improve the stability and robustness of the state estimate.

[0096] Finally, experimental tests were carried out in a three-lane tunnel, and the algorithm was implemented under the ROS (Robot Operating System) framework. The actual lane where the vehicle is located can be observed by playing back the video of the recorded Rosbag data packet. The data of millimeter-wave radars No. 1 and No. 3 were turned off, and at the same time, the sensing ranges of millimeter-wave radars No. 0 and No. 2 were increased to 300 m. The recorded Rosbag data packet after expanding the range of millimeter-wave radars was run with the current traditional Kalman filtering algorithm, and the vehicle tracking situation was observed through Rviz. The millimeter-wave radar data and trajectory estimation data of the misjudged vehicle were exported to the local; in addition, the adaptive Kalman filtering algorithm based on anomaly detection was implemented and run with the same Rosbag data packet in the same test environment, and the vehicle tracking situation was observed through Rviz. The millimeter-wave radar data and trajectory estimation data of the misjudged vehicle were exported to the local; by comparing the estimation results of the two algorithms, the algorithm performance was analyzed. As Figure 6 and Figure 7 , two targets were selected. After adding the adaptive Kalman filtering algorithm, all abnormal estimated values can be corrected.

[0097] The present invention provides a multi-sensor data fusion method based on adaptive Kalman filtering, which is applied to systems such as traffic monitoring, road safety, and intelligent navigation in tunnel scenarios. By collecting and fusing various sensor data in real time, precise tracking, behavior monitoring, and stable estimation of vehicle position, speed, and driving trajectory are realized. In particular, aiming at the special complex interference and data uncertainty in the tunnel environment, through unique data processing and adaptive filtering algorithms, the present invention overcomes the deficiencies of traditional single-sensor systems and conventional data fusion technologies in terms of accuracy and robustness, and ensures the reliable operation of the tunnel vehicle monitoring system. The method of the present invention can be widely applied to multiple key fields such as tunnel safety warning, accident prevention, traffic flow optimization, and emergency response, and has extremely important significance for improving tunnel traffic safety level, traffic efficiency, and emergency response capabilities.

Claims

1. A multi-sensor data fusion method based on adaptive Kalman filtering for vehicle trajectory tracking in tunnel scenarios, characterized in that, It includes the following steps: S1: Deploy sensors in the tunnel scenario and collect raw data, including the collaborative deployment of millimeter-wave radar and geomagnetic sensors. Use the millimeter-wave radar to collect information on the position, speed, and acceleration of the vehicle, and use the geomagnetic sensor to monitor the magnetic field change data generated when the vehicle passes by, and construct the state equation and measurement equation; S2: Preprocess sensor data and perform anomaly detection, including coordinate system conversion, time alignment, and outlier detection; S3: Associate and cluster multi-sensor data. Adopt a multi-object tracking data association method based on the Hungarian algorithm to associate the preprocessed millimeter-wave radar data and geomagnetic sensor data; S4: Adaptive Kalman filter fusion and state estimation. Design an adaptive Kalman filter algorithm based on anomaly detection degree. Quantify the anomaly degree of the measurement by calculating the ratio of the innovation covariance to the actual covariance, and introduce an adaptation factor to adjust the estimation error covariance of the filter in real time to obtain the estimated values of the accurate position and speed of the vehicle.

2. The method according to claim 1, wherein The S1 includes: Deploy a millimeter-wave radar every 150m on the tunnel wall on one side of the tunnel, and deploy a geomagnetic sensor every 15m on the lane lines on both sides of the tunnel. The midpoint of the lane line of the first geomagnetic sensor on the rightmost lane line in the vehicle driving direction is used as the coordinate origin, its X-axis coincides with the center line of the rightmost lane line, and the Y-axis points to the leftmost lane direction.

3. The method according to claim 1, wherein In the S2, for the data collected by the millimeter-wave radar, first perform coordinate system conversion to unify it into the tunnel global coordinate system; align the geomagnetic sensor data collected by the geomagnetic sensor with the millimeter-wave radar data collected by the millimeter-wave radar in time to eliminate the time synchronization error and ensure the consistency of the data in the time dimension; adopt the hypothesis testing method to perform outlier detection on the millimeter-wave radar data and geomagnetic sensor data, and identify and remove the abnormal measurement points by calculating the chi-square threshold.

4. The method according to claim 1, characterized in that In the S3, construct a data association matrix, and the considered factors include the proximity of timestamps, the proximity of spatial positions, and the similarity of motion directions; by solving the data association matrix, realize the correct matching of the measurement data of the same vehicle at different times and on different measurement devices to the corresponding trajectory clusters to solve the problems of data fragmentation and multi-target interference, where the measurement devices include millimeter-wave radar and geomagnetic sensors.

5. The method according to claim 1, wherein In the S4, quantify the anomaly degree of the measurement by calculating the ratio of the innovation covariance to the actual covariance, and introduce an adaptation factor to adjust the estimation error covariance of the filter in real time. When it is detected that there are anomalies in the measurement data, the adaptation factor can automatically reduce the influence of the abnormal data on the filtering result to prevent the filter from diverging.

6. The method according to claim 1, wherein The state equation includes the position and speed state variables of the vehicle. The measurement equation fuses the measurement data of the millimeter-wave radar and the geomagnetic sensor. In the state prediction stage, use the dynamic model of the vehicle to predict the vehicle state at the next moment, and perform state update through adaptive Kalman filter to obtain the estimated values of the accurate position and speed of the vehicle.

Images