An intersection vehicle arrival state perception method considering multi-vehicle information interaction
By constructing a relative observation model between vehicles and an information consistency discrimination mechanism based on the joint prediction error covariance, abnormal observations are identified and suppressed, thus solving the problem of the impact of abnormal observations in multi-vehicle cooperative positioning and achieving higher accuracy and robust state estimation.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- CHONGQING JIAOTONG UNIV
- Filing Date
- 2026-05-08
- Publication Date
- 2026-07-31
AI Technical Summary
In the process of multi-vehicle cooperative localization, existing technologies are unable to effectively identify and suppress the impact of abnormal observations, resulting in insufficient localization accuracy and robustness. In particular, under complex traffic scenarios, the uncertainty of observation data between vehicles is high, making it difficult to achieve stable and consistent state estimation.
By establishing a relative observation model between vehicles and combining the joint prediction error covariance of the target vehicle and neighboring vehicles, the consistency between the current observation and the predicted observation is judged, abnormal observations are screened, and the vehicle state is robustly updated based on the screened valid observations. This constructs an information consistency judgment mechanism based on the joint prediction error covariance to suppress the influence of abnormal observations.
It improves the accuracy, stability and robustness of multi-vehicle cooperative positioning results, avoids the accumulation of estimation bias caused by abnormal observations, and ensures the reliability and accuracy of the reference position estimation of the target vehicle.
Smart Images
Figure CN122493669A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of intelligent transportation and autonomous driving technology, and relates to vehicle cooperative localization and state estimation methods, and particularly to a method for perceiving the arrival state of vehicles at intersections that considers multi-vehicle information interaction. Background Technology
[0002] With the rapid development of intelligent connected vehicle technology and autonomous driving systems, vehicles are placing higher demands on their positioning capabilities, requiring high precision and reliability. Especially in the vehicle-to-everything (V2X) environment, vehicles not only rely on their own sensors for positioning but can also acquire relative observation information through inter-vehicle communication, thereby achieving multi-vehicle collaborative positioning. Compared to single-vehicle positioning methods, collaborative positioning can effectively mitigate problems such as satellite signal obstruction and environmental interference, exhibiting higher positioning accuracy and robustness in complex traffic scenarios.
[0003] In practical applications, vehicle cooperative localization typically relies on observational information such as relative distance and azimuth angle, combined with state estimation methods like Kalman filtering to achieve multi-vehicle state fusion. However, due to the complexity of urban road environments, inter-vehicle observations are easily affected by factors such as occlusion, multipath effects, and sensor errors, leading to significant uncertainty and even outliers in the observation data. Furthermore, in open vehicle-to-everything (V2X) communication environments, observational information may also be affected by data loss, communication delays, or interference from anomalous data, further reducing the stability and reliability of the localization system.
[0004] In existing technologies, distributed Kalman filtering or extended Kalman filtering methods are commonly used to fuse the states of multiple vehicles to improve cooperative localization performance. However, most of these methods assume that the observation noise follows a Gaussian distribution and that the observation data is reliable. In the presence of abnormal observations, this can easily lead to filter divergence or accumulation of estimation bias. Furthermore, some methods use simple threshold judgments or independent residual detection to eliminate abnormal observations, but they do not fully consider the statistical correlation of errors among multiple vehicles, making it difficult to achieve stable and consistent state estimation in complex interaction scenarios.
[0005] Therefore, how to effectively identify and suppress the impact of abnormal observations during multi-vehicle cooperative localization, and at the same time achieve robust state estimation while considering the statistical correlation between vehicles, so as to obtain the intersection arrival state of the target vehicle, has become a key technical problem that urgently needs to be solved. Summary of the Invention
[0006] This invention provides a method for perceiving the arrival status of vehicles at intersections, considering multi-vehicle information interaction. By establishing a relative observation model between vehicles and combining the joint prediction error covariance between the target vehicle and neighboring vehicles, the consistency between the current observation and the predicted observation is judged, abnormal observations are filtered out, and robust updates of vehicle states are performed based on the filtered valid observations. This yields a stable and reliable estimate of the target vehicle's reference position, and ultimately, the distance and position of the target vehicle relative to the intersection reference line, thus determining the target vehicle's intersection arrival status. This method can suppress the impact of abnormal observations on state estimation under conditions such as vehicle occlusion, sensor reflection, multipath error, communication anomalies, or abnormal data interference, improving the accuracy, stability, and robustness of multi-vehicle cooperative localization results.
[0007] In multi-vehicle cooperative localization, the target vehicle needs to integrate relative observation information provided by neighboring vehicles to complete its state update. However, in real traffic environments, relative observations between vehicles are not always reliable. If abnormal observations are directly used for state updates, it can easily lead to the accumulation of estimation biases and even cause filter divergence. To address this technical problem, this invention provides an intersection vehicle arrival state perception method that considers multi-vehicle information interaction. Based on the statistical correlation of errors between the target vehicle and neighboring vehicles, it filters abnormal observations and completes robust updates to the target vehicle's state.
[0008] To address the aforementioned technical problems, this invention proposes a method for perceiving the arrival status of vehicles at intersections that considers multi-vehicle information interaction, comprising the following steps:
[0009] Step 1: Obtain the state information and relative observation information of the target vehicle and neighboring vehicles, and construct the predicted state and observation residual of the target vehicle at the current moment;
[0010] Step 2: Based on the joint error statistics between the target vehicle and neighboring vehicles, perform consistency analysis on the relative observation information to identify abnormal observations;
[0011] Step 3: Based on the abnormal observation identification results, robust processing is performed on the measurement noise covariance corresponding to the abnormal observation, and the robust cooperative state update of the target vehicle is completed based on the robustly processed observation model, and the reference position of the target vehicle is calculated; further, combined with the intersection spatial reference information, the distance position of the target vehicle relative to the intersection reference line is calculated to obtain the intersection arrival state of the target vehicle.
[0012] In this invention, the method for establishing a discrete-time state-space model and predicting states in step 1 includes the following steps:
[0013] Step 11: Create a vehicle At any moment state vector and control input vector They are respectively:
[0014] (1)
[0015] (2)
[0016] in, For vehicles The global position x and y coordinates, For vehicles speed, For vehicles The heading angle, For vehicles The longitudinal acceleration.
[0017] Step 12: Based on the vehicle kinematics, perform discrete propagation of the vehicle state to obtain:
[0018]
[0019] (3)
[0020] In formula (3), This refers to the vehicle's wheelbase. , This represents the vehicle state transition function, used to describe the vehicle. In discrete time, the current state and control input Deducing the state to the next moment The nonlinear kinematic mapping relationship, For vehicle steering angle, The covariance is zero-mean Gaussian white noise. for:
[0021] (4)
[0022] In formula (4), Let f(x) represent the mathematical expectation operator. Equation (4) is used to define the process noise covariance matrix by the expectation of the product of the process noise vector and its transpose.
[0023] Step 13: Using the posterior state estimate from the previous time step, obtain the target vehicle. At any moment Predicted state:
[0024]
[0025] in, For the target vehicle At any moment The predicted state estimate, For the target vehicle At any moment The posterior state estimate, For the target vehicle At any moment The control input vector, This is the vehicle state transition function. This step allows the state estimation result after fusing observations from the previous time step to be recursively applied to the current time step, obtaining the prior state information needed to construct the relative prediction observations.
[0026] (5)
[0027] In formula (5), Indicates the target vehicle At any moment The prediction error covariance matrix, Target vehicle At any moment Posterior error covariance matrix, This represents the first-order Jacobian matrix of the state transition function with respect to the state variables. Let represent the process noise covariance matrix. Equation (5) is used to recursively derive the prediction uncertainty of the target vehicle at the current time based on the posterior estimation result of the previous time step and the system process noise, providing prior statistical information for subsequent relative observation consistency analysis and state update.
[0028] Step 14: Define the target vehicle and neighboring vehicles The state vectors are as follows:
[0029] (6)
[0030] In formula (6), Indicates the target vehicle At any moment The state vector, Indicates nearby vehicles At any moment The state vector, , They represent the target vehicles respectively. The global x and y coordinates, , They represent adjacent vehicles respectively. The global x and y coordinates, , They represent the target vehicles respectively. and neighboring vehicles speed, , They represent the target vehicles respectively. and neighboring vehicles The heading angle. Equation (6) is used to uniformly define the state variables of the vehicle and the adjacent vehicle participating in the relative observation modeling, providing a foundation for subsequent rotation transformation and relative observation prediction model construction.
[0031] Introducing a two-dimensional rotation matrix:
[0032] (7)
[0033] In formula (7), Represents a two-dimensional rotation matrix. The rotation angle is represented by equation (7). Equation (7) is used to transform the position difference in the global coordinate system to the local coordinate system of the target vehicle, thereby achieving a unified expression of the relative observation model. Based on this, the vehicle is constructed. In the vehicle Relative prediction observations in local coordinate system:
[0034] (8)
[0035] In formula (8), Indicates at time Based on the target vehicle and neighboring vehicles The relative predicted observations obtained from the predicted state Represents the relative observation function, Represents the measurement noise vector. , , Representing the local coordinate system respectively direction, The measurement noise components of direction and relative heading angle, where, , Let represent the measurement noise covariance matrix. Equation (8) is used to construct the relative predictive observations of the target vehicle on neighboring vehicles in its own local coordinate system. Essentially, it represents the relative observation results that the system should theoretically obtain under the current predictive state.
[0036] Step 15: In a two-dimensional plane, based on the target vehicle measured by UWB and neighboring vehicles relative distance and relative azimuth Constructing actual relative observables:
[0037] (9)
[0038] In formula (9), Indicates time Target vehicle For nearby vehicles Actual relative observables Indicates the target vehicle The measured distance between it and neighboring vehicles The relative distance between them This represents the relative heading angle obtained by constructing the bidirectional azimuth angle. Indicates nearby vehicles Target vehicle observed The relative azimuth angle, and These represent the relative observations in the target vehicle's local coordinate system. direction and The directional components. In the two-dimensional plane, based on the relative distance and relative azimuth angle measured by UWB, the observation results in polar coordinate form can be converted into relative position observations in a local rectangular coordinate system; at the same time, combined with the bidirectional relative azimuth angle between vehicles, relative heading angle observations can be further constructed. Thus, Equation (9) gives the specific expression of the actual relative observation, providing a basis for subsequent comparison with the predicted relative observation and construction of the innovation vector.
[0039] Step 16: Construct the innovation vector based on the difference between the actual relative observation and the predicted observation:
[0040] (10)
[0041] In formula (10), the expression on the left side is used to indicate that the quantity on the left side is defined by the expression on the right side. Indicates the target vehicle At any moment For nearby vehicles Constructed information vector, Represents actual relative observations, The innovation vector represents the predicted relative observation. Equation (10) is used to characterize the degree of deviation between the current actual observation and the predicted observation. The larger the innovation vector, the higher the degree of inconsistency between the current observation and the prediction; the smaller the innovation vector, the better the consistency between the current observation and the prediction. The innovation vector is used to characterize the degree of deviation between the current observation and the predicted observation.
[0042] In this invention, the method for constructing the joint prediction error covariance matrix and performing consistency judgment in step 2 includes the following steps:
[0043] Step 21: Define the prediction errors for the target vehicle and neighboring vehicles as follows:
[0044] (11)
[0045] In formula (11), Indicates the target vehicle At any moment The prediction error Indicates the target vehicle The true state Indicates the target vehicle The predicted state estimate; Indicates nearby vehicles At any moment The prediction error Indicates nearby vehicles The true state Indicates nearby vehicles The predicted state estimate. Equation (11) is used to define the prediction error between the vehicle and the neighboring vehicle, providing a basis for subsequent linearization of the relative observation function and joint error covariance modeling.
[0046] Step 22: For the observation function exist First-order linearization is performed at the point, and the Jacobian matrix of the observation function with respect to the target vehicle state and the neighboring vehicle states is obtained. and The linearization result is:
[0047] (12)
[0048] In formula (12), For vehicles For vehicles Actual relative observables For the predicted state and The calculated predicted relative observables, and They represent the observation function with respect to the target vehicle. Status and nearby vehicles The first-order Jacobian matrix of the state. Equation (12) is an approximate result obtained by linearizing the observation function in the vicinity of the current predicted state, which is used to establish the linear relationship between the innovation vector and the prediction error of the current vehicle, the prediction error of the neighboring vehicle, and the measurement noise.
[0049] Step 23: Construct a joint prediction error covariance matrix based on the prediction error covariance of the target vehicle, the prediction error covariance of neighboring vehicles, and the cross covariance of their prediction errors.
[0050] (13)
[0051] In formula (13), Indicates the target vehicle and neighboring vehicles At any moment The joint prediction error covariance matrix, Indicates the target vehicle The prediction error covariance matrix, Indicates nearby vehicles The prediction error covariance matrix, and Let represent the cross covariance matrix of the prediction error between the target vehicle and its neighboring vehicles, respectively. Equation (13) is used to jointly characterize the statistical correlation between the prediction errors of the target vehicle and its neighboring vehicles. Compared with the processing method that only uses the error covariance of a single vehicle, it can more accurately reflect the uncertainty propagation relationship under the condition of multi-vehicle state coupling.
[0052] Step 24: Construct the joint observation Jacobian matrix based on the first-order partial derivative matrix of the observation function with respect to the target vehicle state and the states of neighboring vehicles:
[0053] (14)
[0054] In formula (14), The joint observation Jacobian matrix is derived from the local vehicle Jacobian matrix. Jacobian matrix of neighboring cars The results are obtained by splicing together. Equation (14) is used to uniformly represent the sensitivity of the observation function to the state variables of the vehicle and its neighboring vehicles, and provides a matrix basis for the calculation of the information covariance and joint optimal gain.
[0055] Step 25: Calculate the innovation covariance based on the joint prediction error covariance matrix, the joint observation Jacobian matrix, and the measurement noise covariance matrix.
[0056] (15)
[0057] In formula (15), Represents the new information covariance matrix. Let represent the measurement noise covariance matrix. Equation (15) is used to characterize the statistical uncertainty of the innovation vector. This matrix comprehensively considers the combined effects of the prediction errors of the current vehicle and neighboring vehicles, as well as the measurement noise on the current relative observation bias.
[0058] Step 26: Calculate the residual consistency statistic based on the innovation vector and innovation covariance:
[0059] (16)
[0060] In formula (16), Indicates the target vehicle At any moment For nearby vehicles The constructed residual consistency statistic, Represents the innovation vector. Let represent the innovation covariance matrix. Equation (16) essentially gives the squared Mahalanobis distance after covariance normalization of the innovation vector, which is used to measure the statistical consistency between the current actual relative observation and the predicted relative observation. The larger the value, the more significant the deviation between the current observation and the prediction, and the more likely it is to be an anomalous observation; The smaller the value, the better the consistency between the current observation and the prediction. Based on the comparison between this statistic and the preset chi-square threshold, it can be determined whether the current relative observation participates in the state update.
[0061] Step 27: Compare the residual consistency statistic with the preset chi-square threshold; when If the observation is inconsistent with the prediction, it is considered to be inconsistent with the current state update and will not participate in the current state update; when When that time, the current relative observation is determined to be a valid observation.
[0062] In this invention, the method for robustly updating the target vehicle state based on the screening results in step 3 includes the following steps:
[0063] Step 31: Define the joint error vector of the target vehicle and its neighboring vehicles as:
[0064] (17)
[0065] In formula (17), Indicates the target vehicle and neighboring vehicles The joint error vector is formed by the target vehicle prediction error. Prediction error of neighboring vehicles The result is obtained by splicing. Equation (17) is used to unify the state errors of the vehicle and its neighboring vehicles into the same joint vector, so as to facilitate subsequent joint gain design and robust state update.
[0066] Step 32: Calculate the robust innovation covariance based on the joint prediction error covariance matrix, the joint observation Jacobian matrix, and the dilated measurement noise covariance matrix.
[0067] (18)
[0068] In formula (18), Describe the robust new information covariance matrix. Let represent the robust measurement noise covariance matrix after anomaly observation suppression. Equation (18) is used to further fuse the robustly processed measurement noise information on the basis of the joint prediction error covariance, so as to obtain the innovation covariance for robust state update.
[0069] Step 33: Calculate the joint optimal gain based on the robust innovation covariance:
[0070] (19)
[0071] In formula (19), Indicates the target vehicle and neighboring vehicles The corresponding joint optimal gain matrix. Equation (19) is used to calculate the optimal weights for correcting the joint state error based on the joint prediction error covariance, the joint observation Jacobian matrix, and the robust innovation covariance.
[0072] Step 34: Extract the first block of the joint optimal gain corresponding to the target vehicle to obtain the robust gain matrix of the target vehicle:
[0073] (20)
[0074] In formula (20), This represents the target vehicle extracted from the joint optimal gain matrix. The corresponding robust gain matrix. Equation (20) is used to characterize the correction weight of the current effective relative observation on the target vehicle state update. This gain takes into account the prediction error of the target vehicle itself, the prediction error of neighboring vehicles, and the cross-covariance between the two.
[0075] Step 35: Update the target vehicle state using the robust gain matrix to obtain the posterior state estimate of the target vehicle at the current moment:
[0076] (twenty one)
[0077] In formula (21), Indicates the target vehicle At any moment The posterior state estimate, Indicates the target vehicle At any moment The prior state estimate is obtained. Equation (21) is used to correct the predicted state of the target vehicle using the current effective relative observations, and obtain the posterior state estimate after fusing the observation information.
[0078] Step 36: Update the posterior state covariance of the target vehicle based on the robust gain matrix to obtain:
[0079] (twenty two)
[0080] In formula (22), Indicates the target vehicle At any moment The posterior state covariance matrix, Let represent its predicted state covariance matrix. Equation (22) is used to synchronously update the uncertainty of the target vehicle state estimate after the state update is completed, thereby reflecting the change in estimation accuracy after fusing the current effective observations.
[0081] Step 37: Synchronously update the cross covariance between the target vehicle and its neighboring vehicles, obtaining:
[0082] (twenty three)
[0083] In formula (23), Indicates the target vehicle and neighboring vehicles At any moment The posterior cross covariance matrix, Let represent the prediction cross-covariance matrix of the two. Equation (23) is used to synchronously correct the statistical correlation information between the target vehicle and its neighboring vehicles after the state update, so as to ensure the accuracy of the subsequent joint prediction error covariance calculation.
[0084] Step 38: Extract the position component from the posterior state estimate of the target vehicle at the current moment to obtain the reference position estimate of the target vehicle:
[0085] (twenty four)
[0086] In formula (24), Indicates the target vehicle At any moment The reference position estimate, Let represent the position extraction matrix, used to extract position components from the posterior state estimation vector of the target vehicle. Equation (24) is used to extract position components from the posterior state estimation of the target vehicle after robust update, to obtain the reference position estimate of the target vehicle.
[0087] After obtaining the target vehicle reference position estimate, the target vehicle reference position estimate is mapped onto the reference path of the target approach lane. The relative distance of the target vehicle along the current driving direction to the intersection reference line can be calculated, and the intersection arrival status perception result of the target vehicle can be formed accordingly.
[0088] Target vehicle At any moment The reference position is estimated to be Meanwhile, the target reference point on the key reference line of the intersection The unit vector of the driving direction at the target entrance is, in, This represents the tangential direction angle of the reference path along the approach lane where the target vehicle is located. The directed relative distance of the target vehicle along the approach lane to the key reference line of the intersection is defined as...
[0089] (25)
[0090] In formula (25), Indicates the target vehicle At any moment The directed distance relative to the critical reference line at the intersection. When When, it indicates that the target vehicle has not yet reached the key reference line; when When, it indicates that the target vehicle has reached the key reference line; when When the target vehicle has crossed the critical reference line, Equation (25) is used to further convert the target vehicle reference position estimate into a distance position parameter relative to the intersection, thereby providing a basis for determining the arrival status of vehicles at the intersection.
[0091] Compared with the prior art, the present invention has the following advantages:
[0092] This invention constructs an information consistency discrimination mechanism based on joint prediction error covariance to perform consistency analysis on relative observations between vehicles. This effectively identifies anomalous observations caused by occlusion, multipath effects, perception errors, and communication anomalies, preventing anomalous observations from directly participating in state updates and causing accumulated estimation bias. Introducing the prediction error cross-covariance between the target vehicle and neighboring vehicles during the observation screening process more accurately characterizes the statistical correlation between the states of multiple vehicles. Compared to methods based solely on single-vehicle residuals or independent observations, this improves the accuracy of anomalous observation identification and the consistency of state estimation. This invention performs measurement noise covariance dilation processing on anomalous observations to achieve equivalent elimination of anomalous observations. Based on valid observations, it robustly updates the target vehicle state, state covariance, and cross-covariance, improving the estimation accuracy, stability, and robustness of multi-vehicle cooperative positioning under unreliable observation conditions. The reference position estimation results obtained by this invention do not depend on single observation data but are generated jointly by valid observations after consistency screening. This avoids anomalous observations skewing the reference position estimation, improving the reliability and engineering application value of the reference position estimation. Attached Figure Description
[0093] Figure 1 This is a flowchart of the method of the present invention;
[0094] Figure 2 This is a diagram illustrating the lateral offset of the vehicle.
[0095] Figure 3 This is a schematic diagram of an embodiment. Detailed Implementation
[0096] The following is in conjunction with the appendix Figure 1-3The present invention will be further described in detail with reference to the embodiments, but the implementation of the present invention is not limited thereto. The implementation of the present invention is not limited to the embodiments described herein, and any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and are included within the protection scope of the present invention.
[0097] Figure 2 This invention demonstrates the applicable scenario for vehicle-road cooperative systems at intersections. In this scenario, the target vehicle can integrate relevant observation information provided by neighboring vehicles and roadside units. However, due to occlusion, multipath propagation, perception errors, communication anomalies, or interference from abnormal data, some observations may be inconsistent with the actual situation. To address these issues, this invention identifies abnormal observations through relative observation consistency discrimination and completes robust cooperative localization based on valid observations.
[0098] A method for perceiving vehicle arrival status at an intersection that considers multi-vehicle information interaction includes the following steps:
[0099] Step 1: Obtain the state information and relative observation information of the target vehicle and neighboring vehicles, and construct the predicted state and observation residual of the target vehicle at the current moment;
[0100] Step 2: Based on the joint error statistics between the target vehicle and neighboring vehicles, perform consistency analysis on the relative observation information to identify abnormal observations;
[0101] Step 3: Based on the anomaly observation identification results, robust processing is performed on the observation information, and robust cooperative state update of the target vehicle is completed based on the processed observation information, calculating the reference position of the target vehicle. Further, combined with the intersection spatial reference information, the distance and position of the target vehicle relative to the intersection reference line are calculated to obtain the intersection arrival state of the target vehicle.
[0102] Figure 2 This demonstrates the applicable scenario for vehicle-road cooperative systems at intersections. In this scenario, the target vehicle can be fused in step 1 to obtain the state information and relative observation information of the target vehicle and neighboring vehicles, and to construct the predicted state and observation residual of the target vehicle at the current moment. In the embodiment, the selected moment... The target vehicle 1 is selected as the vehicle to be located, and neighboring vehicles 2 to 8 within its communication radius are selected as cooperative observation vehicles. The target vehicle and all neighboring vehicles use a unified state representation, with state variables including global coordinate position, velocity, and heading angle. Based on the discrete-time kinematic model, the kinematics of target vehicle 1 and neighboring vehicles 2 to 8 at time... The state is propagated to obtain the corresponding state estimate. As shown in Tables 1-3, target vehicle 1 at time... The state estimate is Each of the adjacent vehicles 2 to 8 has its own corresponding state estimate. For example... Figure 3 As shown, the target vehicle can obtain relative distance and relative orientation observations of neighboring vehicles in the local coordinate system; among them, normal observations show good consistency with the target vehicle's reference position, while abnormal observations show significant deviations. Based on this, relative prediction observations, actual relative observations, and innovation vectors can be further constructed.
[0103] After obtaining the state estimation results of the target vehicle and neighboring vehicles, using the local coordinate system of the target vehicle as a reference, and based on the relative positional and heading relationships between the target vehicle and each neighboring vehicle, the relative predicted observations of the target vehicle relative to each neighboring vehicle are constructed. On the other hand, based on the relative distance, relative orientation, and other measurement information obtained from the onboard ultra-wideband sensor, the actual relative observations of the target vehicle relative to each neighboring vehicle are constructed. Then, based on the difference between the actual relative observations and the relative predicted observations, the observation residual, i.e., the innovation vector, is constructed. Tables 1-3 show the actual relative observations, relative predicted observations, and innovation vectors between target vehicle 1 and neighboring vehicles 2 to 8. Taking neighboring vehicle 2 as an example, the actual relative observation of target vehicle 1 relative to neighboring vehicle 2 is as follows: The corresponding relative prediction observations are Thus, the new information vector is obtained as Similarly, the observation residuals for target vehicle 1 and its neighboring vehicles 3 to 8 can be obtained respectively, which can be used to characterize the degree of deviation between the actual observation and the predicted observation at the current moment, and provide a basis for subsequent consistency analysis.
[0104] Table 1. Predicted State Estimation Results of the Target Vehicle and Neighboring Vehicles
[0105]
[0106] Table 2 Actual and predicted relative observation results between the target vehicle and neighboring vehicles
[0107]
[0108] Table 3. Observational information vectors of the target vehicle to neighboring vehicles
[0109]
[0110] Step 2: Based on the joint error statistical relationship between the target vehicle and neighboring vehicles, a consistency analysis is performed on the relative observation information to identify abnormal observations. In this embodiment, firstly, a corresponding innovation covariance matrix is constructed based on the prediction error covariance of target vehicle 1, the prediction error covariances of neighboring vehicles 2 to 8, and the cross covariance of prediction errors between the target vehicle and neighboring vehicles. When the stability calculation conditions are met, a cross covariance term is further introduced for joint error statistical analysis. Then, based on the innovation vector and the innovation covariance matrix, the residual consistency statistics corresponding to the relative observations of each neighboring vehicle are calculated. In this embodiment, the residual consistency statistic is constructed using the squared Mahalanobis distance form, with a measurement dimension of 3. The preset chi-square threshold is the threshold corresponding to 3 degrees of freedom and a confidence level of 95%, i.e. .
[0111] Table 4 presents the statistical analysis results of consistency between target vehicle 1 and neighboring vehicles 2 to 8. As shown in Table 2, the residual consistency statistics for the observations corresponding to neighboring vehicles 2, 3, 4, and 5 are 1.4911, 6.6364, 4.4224, and 2.2131, respectively, all below the preset threshold of 7.815. Therefore, these observations are considered statistically consistent with the predicted state and are valid observations. The residual consistency statistics for the observations corresponding to neighboring vehicles 6, 7, and 8 are 16.295, 10.169, and 11.807, respectively, all exceeding the preset threshold of 7.815. Therefore, these observations are considered anomalous observations and do not directly participate in the current state update. This completes the identification and screening of anomalous observations.
[0112] Table 4. Relative observation consistency statistics and anomaly identification results
[0113]
[0114] Step 3: Based on the anomaly observation identification results, robust processing is performed on the observation information, and robust cooperative state update of the target vehicle is completed based on the processed observation information to obtain the reference position estimate of the target vehicle. In the embodiment, for the anomaly observations identified in Step 2, the corresponding measurement noise covariance is dilated to reduce the weight of such observations in subsequent state updates until it approaches zero; for valid observations that pass the consistency judgment, the robust gain matrix corresponding to the target vehicle at the current time is calculated based on the target vehicle prediction error covariance, joint observation statistics, and processed measurement noise covariance, and state correction and covariance update are completed accordingly.
[0115] Table 5 presents the robust processing and robust update results for target vehicle 1 during the fusion of observations from neighboring vehicles 2 to 8. As shown in Table 3, in the first to fourth updates, the observations corresponding to neighboring vehicles 2, 3, 4, and 5 were all determined to be valid observations and participated in the target vehicle state correction. Specifically, after fusing the observations corresponding to neighboring vehicle 2, the target vehicle state correction amount is... The updated status is The state covariance trace decreased from the initial value of 5.6765 to 3.9701; after further fusing the observations corresponding to the three neighboring vehicles, the state of the target vehicle was further corrected to... The state covariance trace decreased to 3.7981; after fusing the corresponding observations of 4 neighboring vehicles, the target vehicle state was updated to... The state covariance trace decreased to 3.6220; after fusing the observations of 5 neighboring vehicles, the target vehicle state was updated to... The state covariance trace continued to decrease to 3.4584.
[0116] Table 5. Robust handling and robust state update results after effective observation screening.
[0117]
[0118] For the abnormal observations identified in step 2, namely the observations corresponding to neighboring vehicle 6 and neighboring vehicle 7, the expansion coefficients in Table 5 are taken as follows: Since the observations of neighboring vehicle 8 do not participate in the target vehicle state correction, their corresponding state corrections are all zero, and the target vehicle state remains unchanged. For neighboring vehicle 8, the corresponding observations, after robust processing in the current update sequence, continue to participate in robust updates, resulting in a state correction of [value missing]. The updated target vehicle status is The state covariance trace was further reduced to 3.2525. This demonstrates that by suppressing outlier observations and weighted fusion of valid observations, the uncertainty in target vehicle state estimation is continuously reduced.
[0119] Table 6 shows the time of target vehicle 1. The reference position is as shown in Table 4. After fusing effective relative observations from multiple neighboring vehicles and completing robust cooperative state updates, the final reference position of target vehicle 1 is estimated to be... The corresponding speed estimate is 4.5355, the heading angle estimate is 2.4648, and the final state covariance trace is 3.2525. This reference position estimate is not directly based on the target vehicle's own broadcast position, but is formed by the relative observations of multiple neighboring vehicles after consistency discrimination, outlier observation screening, and robust fusion. Therefore, even in the presence of measurement noise, drift bias, outlier broadcasts, or malicious interference, it can still provide a relatively stable and reliable position reference result for the target vehicle.
[0120] Table 6. Estimation results of the final reference position of the target vehicle
[0121]
[0122] In summary, this embodiment completes the target vehicle reference position estimation process through state information acquisition and observation residual construction in step 1, consistency analysis and anomaly observation identification in step 2, and robust processing and robust collaborative state update in step 3. Furthermore, the relative distance between the vehicle and the intersection is obtained through Table 7.
[0123] Table 7. Relative distance of the target vehicle to the intersection
[0124]
[0125] As can be seen from Tables 1 to 4, this method can effectively distinguish between valid observations and abnormal observations, and gradually reduce the uncertainty of target vehicle state estimation under the condition of multi-neighbor vehicle cooperation, thereby improving the stability and reliability of the positioning results.
Claims
1. A method for intersection vehicle arrival state perception considering multi-vehicle information interaction, characterized in that, Includes the following steps: Step 1: Obtain the state information and relative observation information of the target vehicle and neighboring vehicles, and construct the predicted state and observation residual of the target vehicle at the current moment; Step 2: Based on the joint error statistics between the target vehicle and neighboring vehicles, perform consistency analysis on the relative observation information to identify abnormal observations; Step 3: Based on the anomaly observation identification results, robust processing is performed on the observation information, and the robust cooperative state update of the target vehicle is completed based on the processed observation information, and the reference position of the target vehicle is calculated; further, combined with the intersection spatial reference information, the distance and position of the target vehicle relative to the intersection reference line are calculated to obtain the intersection arrival state of the target vehicle.
2. The method for perceiving the arrival status of vehicles at an intersection considering multi-vehicle information interaction according to claim 1, characterized in that, Step 1 involves acquiring the state information and relative observation information of the target vehicle and neighboring vehicles, and constructing the predicted state and observation residual of the target vehicle at the current moment. This includes the following steps: Step 11: Create a vehicle At any moment The state vector and control input vector; Step 12: Discretely propagate the vehicle state based on the vehicle kinematics. Step 13: Using the posterior state estimate from the previous time step, obtain the target vehicle. At any moment The predicted state; Step 14: Define the target vehicle and neighboring vehicles The state vector; Step 15: In the two-dimensional plane, calculate the relative distance based on the UWB measurement. and relative azimuth Construct actual relative observables; Step 16: Construct the innovation vector based on the difference between the actual relative observation and the predicted observation: (1) In formula (1), Indicates the target vehicle At any moment For nearby vehicles Constructed information vector, Represents actual relative observations, The information vector represents the predicted relative observation; the information vector is used to characterize the degree of deviation of the current observation from the predicted observation.
3. The method for perceiving the arrival status of vehicles at an intersection considering multi-vehicle information interaction according to claim 1, characterized in that, In step 2, based on the joint error statistical relationship between the target vehicle and neighboring vehicles, a consistency analysis is performed on the relative observation information to identify abnormal observations, including the following steps: Step 21: Define the prediction error between the target vehicle and neighboring vehicles; Step 22: For the observation function exist Linearization is performed at this point; Step 23: Construct a joint prediction error covariance matrix based on the prediction error covariance of the target vehicle, the prediction error covariance of neighboring vehicles, and the prediction error cross covariance between the two. Step 24: Construct the joint observation Jacobian matrix based on the first-order partial derivative matrix of the observation function with respect to the target vehicle state and the neighboring vehicle states; Step 25: Calculate the innovation covariance based on the joint prediction error covariance matrix, the joint observation Jacobian matrix, and the measurement noise covariance matrix. (2) In formula (2), Represents the new information covariance matrix. Denotes the joint observation Jacobian matrix. Indicates the target vehicle and neighboring vehicles At any moment The joint prediction error covariance matrix, Denotes the joint observation Jacobian matrix as transposed. The measurement noise covariance matrix is represented by equation (2). Equation (2) is used to characterize the uncertainty of the innovation vector in a statistical sense, taking into account the combined effects of the prediction error of the target vehicle and the neighboring vehicles as well as the measurement noise on the current relative observation bias. Step 26: Calculate the residual consistency statistic based on the innovation vector and innovation covariance: (3) In formula (3), Indicates the target vehicle At any moment For nearby vehicles The constructed residual consistency statistic, Represents the innovation vector. Let represent the innovation covariance matrix; Equation (3) gives the squared Mahalanobis distance after covariance normalization of the innovation vector, which is used to measure the statistical consistency between the current actual relative observation and the predicted relative observation. Step 27: Compare the residual consistency statistic with the preset chi-square threshold: when When the current observation is inconsistent with the prediction, it is considered that the current observation is inconsistent and will not participate in the current state update; when When that time, the current relative observation is determined to be a valid observation.
4. The method for perceiving the arrival status of vehicles at an intersection considering multi-vehicle information interaction according to claim 1, characterized in that, In step 3, based on the anomaly observation identification results, robust processing is performed on the observation information, and robust cooperative state update of the target vehicle is completed based on the processed observation information to obtain the reference position estimate of the target vehicle, including the following steps: Step 31: Define the joint error vector between the target vehicle and neighboring vehicles; Step 32: Calculate the robust innovation covariance based on the joint prediction error covariance matrix, the joint observation Jacobian matrix, and the dilated measurement noise covariance matrix. (4) In formula (4), Describe the robust new information covariance matrix. The robust measurement noise covariance matrix after anomaly observation suppression is represented; Equation (4) is used to further fuse the robustly processed measurement noise information on the basis of the joint prediction error covariance, so as to obtain the new information covariance for robust state update; Step 33: Calculate the joint optimal gain based on the robust innovation covariance: (5) In formula (5), Indicates the target vehicle and neighboring vehicles The corresponding joint optimal gain matrix; Equation (5) is used to calculate the optimal weight of the current relative observation for joint state error correction based on the joint prediction error covariance, the joint observation Jacobian matrix and the robust innovation covariance. Step 34: Extract the first block of the joint optimal gain corresponding to the target vehicle to obtain the robust gain matrix of the target vehicle; Step 35: Update the target vehicle state using the robust gain matrix to obtain the posterior state estimate of the target vehicle at the current moment; Step 36: Update the posterior state covariance of the target vehicle according to the robust gain matrix; Step 37: Synchronously update the cross-covariance between the target vehicle and its neighboring vehicles; Step 38: Extract the position component from the posterior state estimate of the target vehicle at the current moment to obtain the reference position of the target vehicle: (6) In formula (6), Indicates the target vehicle At any moment The reference position estimate, Indicates the target vehicle At any moment The posterior state estimate, The position extraction matrix is used to extract position components from the posterior state estimation vector of the target vehicle; Equation (6) is used to extract position components from the posterior state estimation of the target vehicle after robust update, to obtain the reference position of the target vehicle. After obtaining the reference position estimation of the target vehicle, the relative distance of the target vehicle along the current driving direction to the intersection reference line is calculated by mapping the reference position estimation of the target vehicle to the reference path of the target entrance lane, and the intersection arrival state perception result of the target vehicle is formed accordingly.