A vehicle cooperative positioning system for traversing continuous blind spots

Abstract

This invention discloses a vehicle cooperative positioning system for traversing continuous blind zones, comprising a module for acquiring multi-source observation data, a module for constructing a spatiotemporally consistent cooperative factor graph, a cooperative constraint optimization module, an adaptive cooperative strategy adjustment module, and a cooperative information re-fusion module. This invention belongs to the field of data processing technology, specifically a vehicle cooperative positioning system for traversing continuous blind zones. This scheme employs observability-driven cooperative constraint construction and spatiotemporally consistent cooperative factor graph modeling, maintaining high observability and stable positioning even in scenarios without satellite signals, achieving unified fusion of multi-source heterogeneous data. It utilizes communication quality-driven adaptive cooperative switching and predictive prior constraints for delay-compensated re-fusion, achieving autonomous positioning without drift during communication interruptions and unified global trajectory calibration after communication recovery, providing high-precision and continuous positioning support for traversing continuous blind zones.

Description

Technical Field

[0001] This invention belongs to the field of data processing technology, specifically a vehicle cooperative positioning system for traversing continuous blind spots. Background Technology

[0002] The vehicle cooperative positioning system is an intelligent system that relies on vehicle network communication, integrates multi-source positioning and sensing data from Beidou satellite positioning, multiple vehicles, roadside facilities, and the cloud, and achieves high-precision, high-reliability, and continuous vehicle positioning through distributed and centralized information interaction and algorithm fusion. Typical vehicle cooperative positioning systems suffer from technical problems such as weak system observability and severe inertial positioning drift due to GNSS failure during continuous blind zone crossings, and difficulty in unifying and integrating spatiotemporally heterogeneous multi-source cooperative observations. In continuous blind zone scenarios with communication obstruction and network interruption, unstable V2X communication leads to cooperative positioning failure and trajectory breakage. After communication is restored, historical errors cannot be backtracked and corrected, resulting in insufficient positioning continuity and robustness. Summary of the Invention

[0003] To address the aforementioned issues and overcome the shortcomings of existing technologies, this invention provides a vehicle cooperative positioning system for traversing continuous blind zones. Specifically, it addresses the technical problems of weak observability and severe inertial positioning drift caused by GNSS failure during continuous blind zone traversal, as well as the difficulty in unifying and fusing spatiotemporally heterogeneous multi-source cooperative observations. This system employs observability-driven cooperative constraint construction and spatiotemporally consistent cooperative factor graph modeling, maintaining high observability and stable positioning even in scenarios without satellite signals, achieving unified fusion of multi-source heterogeneous data. Furthermore, it addresses the technical problems of insufficient positioning continuity and robustness caused by unstable V2X communication leading to cooperative positioning failure and trajectory breaks in continuous blind zone scenarios with communication obstruction and network interruptions, where historical errors cannot be backtracked and corrected after communication recovery. This system utilizes communication quality-driven adaptive cooperative switching and predictive prior constraints for delay-compensated re-fusion, achieving autonomous positioning without drift during communication interruptions and unified global trajectory calibration after communication recovery, providing high-precision and continuous positioning support for traversing continuous blind zones.

[0004] The present invention provides a vehicle cooperative positioning system for continuous blind zone crossing, including a multi-source observation data acquisition module, a spatiotemporal consistency cooperative factor graph construction module, a cooperative constraint optimization module, an adaptive cooperative strategy adjustment module, and a cooperative information re-fusion module; The module for acquiring multi-source observation data specifically involves collecting multi-source observation data and performing data preprocessing to obtain preprocessed multi-source observation data. The module for constructing a spatiotemporally consistent collaborative factor graph specifically involves constructing a unified state node system and four types of factors based on preprocessed multi-source observation data, performing spatiotemporal calibration and factor weight allocation, and obtaining a spatiotemporally consistent collaborative factor graph. The collaborative constraint optimization module specifically evaluates the contribution of collaborative node observations, selects the optimal collaborative node, optimizes the dynamic weight of factors, quantifies the system observability index, and iteratively optimizes the vehicle status node to obtain continuous positioning results that meet the preset accuracy. The adaptive collaborative strategy adjustment module specifically constructs a communication state hierarchical judgment model, performs adaptive adjustment of collaborative weights based on the current communication state level and continuous positioning results that meet the preset accuracy, and introduces a transition smoothing algorithm to achieve smooth control of mode switching. Specifically, when the adaptive collaborative strategy adjustment module determines that the communication state has recovered to a good state, the delayed collaborative observation data is preprocessed and reintroduced into the spatiotemporal consistency collaborative factor graph for global optimization. Historical collaborative information is introduced to construct predictive collaborative prior constraints, and then the system state is calibrated to obtain the calibrated high-precision vehicle collaborative positioning result.

[0005] Furthermore, the module for acquiring multi-source observation data includes the following: Real-time acquisition of multi-source observation data, including data from vehicle-mounted sensors, collaborative observation data, and communication status information; to obtain raw multi-source observation data. Data preprocessing involves cleaning, synchronizing, and unifying the coordinates of the collected raw multi-source observation data to obtain preprocessed multi-source observation data.

[0006] Furthermore, the module for constructing a spatiotemporally consistent cooperating factor graph includes the following: Based on the preprocessed multi-source observation data, a unified state node system is constructed; each vehicle corresponds to a set of independent state nodes. Based on a unified state node system and the constraint characteristics of multi-source observation data, four types of factors are constructed, all of which directly affect the corresponding state nodes. The four types of factors include IMU pre-integration factor, cooperative relative constraint factor, vehicle odometer factor, and environmental closed-loop detection factor. Spatiotemporal consistency integration is achieved by associating four types of factors with state nodes through the topological structure of the factor graph. Spatiotemporal calibration formulas are calculated for the differences in time scale and spatial precision of different factors to achieve spatiotemporal alignment of multi-source observation data. At the same time, a probabilistic association model between factors and state nodes is established to transform the noise characteristics of various observations into the weight parameters of factors and calculate the factor weight allocation formula. By using state nodes as variable nodes in the factor graph and four types of factors as constraint factors, and combining spatiotemporal calibration relationships and weight allocation results, the topological connection and spatiotemporal association between nodes and factors are completed, and finally a complete spatiotemporally consistent collaborative factor graph is generated.

[0007] Furthermore, the collaborative constraint optimization module includes the following: Define a collaborative node, which refers to the entity that provides observation information for the current vehicle, participates in collaborative positioning constraints, and is dynamically evaluated and screened in continuous GNSS blind zone scenarios. Specifically, it includes vehicle-mounted collaborative nodes and roadside collaborative nodes. The contribution of collaborative node observations is evaluated by constructing a collaborative node observation contribution evaluation model to dynamically assess the contribution of observation information from surrounding vehicles and roadside units to the current vehicle's positioning. Evaluation indicators include the accuracy of observation data, data update frequency, link stability, and the relative position of collaborative nodes and the current vehicle. The optimal collaborative node selection is based on the contribution evaluation results. An optimal collaborative node screening strategy is implemented, and collaborative nodes are quickly clustered and screened according to a clustering algorithm. A dynamic node update mechanism is set up to re-evaluate the contribution of collaborative nodes every preset period. The set of nodes participating in collaboration is dynamically adjusted according to changes in vehicle movement status and communication environment, and finally the optimal collaborative nodes are obtained after screening. Adaptive confidence weight optimization introduces an adaptive confidence weight mechanism to dynamically allocate weights to four types of factors, taking into account the reliability differences of different observation constraints. The dynamic weight allocation is adaptively adjusted based on the real-time quality of the observation data to form adaptive dynamic weights. Observability enhancement optimization: Based on the selected optimal collaborative nodes and adaptive dynamic weights, an observability-enhanced collaborative constraint system is constructed to quantify the observability index of the system. Based on the system observability index, the vehicle state nodes are iteratively optimized using the factor graph optimization algorithm, and the iterative optimization objective function is calculated. Through iterative optimization, the positioning drift caused by IMU angular velocity and acceleration offset is gradually suppressed, ensuring that the vehicle positioning accuracy remains within the preset range in continuous GNSS blind zones, and obtaining continuous positioning results that meet the preset accuracy.

[0008] Furthermore, the adaptive collaborative strategy adjustment module includes the following: Real-time communication status determination: Based on the communication status information output by the multi-source observation data module, a communication status classification determination model is constructed. According to the comprehensive communication quality score, the communication status is divided into three levels: good communication, weak communication, and communication interruption. The current communication status level is output in real time. The graded collaborative strategy is adjusted based on the current communication status level and the continuous positioning results that meet the preset accuracy. The collaborative positioning mode and factor weights are dynamically adjusted, and the communication quality and collaborative weights are dynamically bound through adaptive adjustment of collaborative weights. Mode switching smooth control, during the switching process of different cooperative modes, takes the continuous positioning results that meet the preset accuracy as the benchmark, introduces a transition smoothing algorithm, and performs fusion calibration on the vehicle cooperative positioning results before and after the switch.

[0009] Furthermore, the collaborative information re-fusion module includes the following: Collaborative data recovery and preprocessing: When the adaptive collaborative strategy adjustment module determines that the communication status has recovered to a good state, it first receives the delayed collaborative observation data. The collaborative observation data represents the relative observation data obtained from surrounding vehicle-mounted collaborative nodes through V2V communication in the multi-source observation data. Targeted preprocessing is performed on the delayed collaborative observation data, including time alignment and delay compensation. Based on the timestamp record of the communication interruption, the time deviation of the delayed data is corrected, and the delay compensation formula is calculated. By using time delay compensation, the delayed collaborative observation data is precisely time-aligned with the historical data of vehicle autonomous positioning. At the same time, noise filtering and reliability verification are performed on the recovered data to obtain preprocessed delayed collaborative observation data. The preprocessed delayed collaborative observation data is reintroduced into the spatiotemporal consistency collaborative factor graph to supplement the collaborative constraint information during the communication interruption period. Based on the preprocessed delayed collaborative observation data, a delayed collaborative relative constraint factor is constructed. The topology of the spatiotemporal consistency collaborative factor graph is adjusted according to the time characteristics of this factor, and the delayed collaborative relative constraint factor is associated with the vehicle status nodes of the corresponding time period. Global optimization is performed based on the spatiotemporal consistency coordination factor graph after supplementing the delay coordination relative constraint factor. The global optimization algorithm is executed to uniformly iterate and optimize the historical vehicle state nodes and the current vehicle state during the communication interruption. During the optimization process, multi-source constraint information is fully utilized, and predictive coordination prior constraints are introduced to predict the state of coordination nodes. The state sequence of coordination nodes during the communication interruption is predicted time by time. The predicted state is used as the prior expectation to construct predictive prior constraint factors. The predictive prior constraint factors are added to the spatiotemporal consistency coordination factor graph to form predictive coordination prior constraints, which help to correct historical trajectory errors. Then, by minimizing the state estimation error, which includes pose estimation error, velocity estimation error, and IMU bias estimation error, the positioning drift generated during the communication interruption is corrected, realizing the error backtracking correction of the historical trajectory. After system state calibration and global optimization are completed, the system state is fully calibrated, the current state parameters of the vehicle are updated, the global consistency of the system state is restored, and the calibrated high-precision vehicle cooperative positioning results and globally consistent positioning trajectory are obtained.

[0010] The beneficial effects achieved by adopting the above solution are as follows: (1) To address the technical problems of weak observability and severe inertial positioning drift caused by GNSS failure during continuous blind zone crossing, and the difficulty in unifying and integrating spatiotemporally heterogeneous multi-source collaborative observations, observability-driven collaborative constraint construction and spatiotemporally consistent collaborative factor graph modeling are adopted. Even in the absence of satellite signals, high observability and stable positioning can still be maintained, and the unified integration of multi-source heterogeneous data can be achieved. (2) In the case of continuous blind zone scenarios with communication obstruction and network interruption, V2X communication instability leads to failure of cooperative positioning and trajectory breakage. After communication is restored, historical errors cannot be backtracked and corrected, resulting in insufficient positioning continuity and robustness. Adaptive cooperative switching driven by communication quality and predictive prior constraints are adopted to perform time delay compensation re-fusion, so as to achieve autonomous positioning without drift when communication is interrupted and unified global trajectory calibration after communication is restored, providing high-precision and continuous positioning support for continuous blind zone crossing. Attached Figure Description

[0011] Figure 1 This is a schematic diagram of a vehicle cooperative positioning system for traversing continuous blind spots, provided by the present invention.

[0012] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the embodiments of the invention to explain the invention and do not constitute a limitation thereof. Detailed Implementation

[0013] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0014] In the description of this invention, it should be understood that the terms "upper", "lower", "front", "rear", "left", "right", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention.

[0015] Example 1, see Figure 1 The present invention provides a vehicle cooperative positioning system for continuous blind zone crossing, including a multi-source observation data acquisition module, a spatiotemporal consistency cooperative factor graph construction module, a cooperative constraint optimization module, an adaptive cooperative strategy adjustment module, and a cooperative information re-fusion module; The module for acquiring multi-source observation data specifically involves collecting multi-source observation data and performing data preprocessing to obtain preprocessed multi-source observation data. The module for constructing a spatiotemporally consistent collaborative factor graph specifically involves constructing a unified state node system and four types of factors based on preprocessed multi-source observation data, performing spatiotemporal calibration and factor weight allocation, and obtaining a spatiotemporally consistent collaborative factor graph. The collaborative constraint optimization module specifically evaluates the contribution of collaborative node observations, selects the optimal collaborative node, optimizes the dynamic weight of factors, quantifies the system observability index, and iteratively optimizes the vehicle status node to obtain continuous positioning results that meet the preset accuracy. The adaptive collaborative strategy adjustment module specifically constructs a communication state hierarchical judgment model, performs adaptive adjustment of collaborative weights based on the current communication state level and continuous positioning results that meet the preset accuracy, and introduces a transition smoothing algorithm to achieve smooth control of mode switching. Specifically, when the adaptive collaborative strategy adjustment module determines that the communication state has recovered to a good state, the delayed collaborative observation data is preprocessed and reintroduced into the spatiotemporal consistency collaborative factor graph for global optimization. Historical collaborative information is introduced to construct predictive collaborative prior constraints, and then the system state is calibrated to obtain the calibrated high-precision vehicle collaborative positioning result.

[0016] Example 2, see Figure 1 This embodiment is based on the above embodiment. Further, the module for acquiring multi-source observation data includes the following: Real-time acquisition of multi-source observation data includes onboard sensor data (including angular velocity and acceleration data from the inertial measurement unit (IMU), point cloud data from the lidar, and image frame data from the vision sensor), with the acquisition frequency dynamically adapted according to the sensor type (IMU ≥ 100Hz, lidar ≥ 10Hz, vision sensor ≥ 15Hz); collaborative observation data is acquired through the V2X (vehicle-to-everything) communication link, including relative ranging data between vehicles (V2V) (acquired via UWB or millimeter-wave radar) and relative attitude information, as well as roadside unit observation data between vehicles and the road (V2I); and real-time monitoring of communication status information, including V2X link transmission latency, data packet loss rate, available bandwidth, and link connection status, with a sampling interval not exceeding 100ms, to obtain raw multi-source observation data, which includes onboard sensor data, collaborative observation data, and communication status information. Data preprocessing involves cleaning, time synchronization, and coordinate unification of the collected raw multi-source observation data to eliminate data noise and heterogeneity, resulting in preprocessed multi-source observation data, including the following: Data cleaning is performed to remove abnormal data caused by sensor failures and communication interference (such as holes in the LiDAR point cloud, IMU data jumps, and communication data errors). At the same time, a CNN feature enhancement model is used to enhance the features of blurry visual images and sparse LiDAR point clouds to improve data quality. Time synchronization is performed using the timestamp of the vehicle-mounted IMU as a reference. A timestamp alignment algorithm is used to correct the time deviation of LiDAR, visual sensor, and collaborative observation data, ensuring the consistency of various data in the time dimension, and controlling the time synchronization error within 1ms. Coordinate unification is performed by converting all data to the ENU coordinate system, eliminating coordinate system differences between different sensors, vehicles, and roadside units, generating a standardized observation data matrix, clarifying the data format, accuracy indicators, and storage specifications, and directly outputting it to the module for constructing a spatiotemporal consistency co-factor graph.

[0017] Example 3, see Figure 1 This embodiment is based on the above embodiment. The module for constructing a spatiotemporally consistent collaborative factor graph is used to achieve the fusion of multi-source heterogeneous observation information in scenarios with continuous blind spots. It unifies the expression of various constraint information from vehicle-mounted autonomous perception and vehicle-to-vehicle / vehicle-road cooperative observation, solving the problems of spatiotemporal heterogeneity and dispersed constraint relationships of multi-source observation data. It includes the following: Based on preprocessed multi-source observation data, a unified state node system is constructed. This unified state node system represents a state management system that abstracts and standardizes multi-source heterogeneous sensing and operational data within the system to form state nodes with a unified format. Through hierarchical division, correlation modeling, and global management, a globally observable and coordinated scheduling state management system is constructed. The state nodes are the key state parameters to be estimated in the system, mainly including vehicle pose state nodes (3D position, 3D attitude angle), velocity state nodes (3D linear velocity), and IMU bias state nodes (accelerometer bias, gyroscope bias). Each vehicle corresponds to a set of independent state nodes. Roadside units only serve as observation sources to provide constraints and do not have independent state nodes. All state nodes are modeled using continuous time, supporting dynamic incremental updates to adapt to dynamic scenarios of high-speed vehicle movement and continuous blind spot crossing. Based on a unified state node system and the constraint characteristics of multi-source observation data, four types of factors are constructed. All factors directly affect the corresponding state nodes, realizing the spatiotemporal consistency fusion of different observation information and state nodes. The four types of factors include IMU pre-integration factor, cooperative relative constraint factor, vehicle odometer factor, and environmental closed-loop detection factor. The IMU pre-integration factor, based on the angular velocity and acceleration data of the IMU, calculates the changes in the vehicle's pose and velocity at adjacent moments through a pre-integration algorithm, constructs inertial navigation constraints, and directly applies them to the vehicle's pose state node, velocity state node, and IMU offset state node, serving as the basic constraints for positioning in blind zones and filling the gaps caused by missing GNSS signals. The cooperative relative constraint factor, based on vehicle-to-vehicle / vehicle-to-infrastructure cooperative observation data, constructs relative position constraints and relative attitude constraints between vehicles and neighboring vehicles, and between vehicles and roadside units. It acts on the pose state nodes of different vehicles, transforms cooperative observation information into constraint relationships in the factor graph, and strengthens the observation dimension in blind spots. The vehicle odometer factor, based on vehicle motion information obtained by lidar point cloud matching and visual feature matching, constructs local motion constraints, which are applied to the vehicle pose state node and velocity state node to capture the instantaneous motion state of the vehicle and suppress positioning drift in a short period of time. The environmental closed-loop detection factor detects whether the vehicle has passed through the observed area by feature matching of LiDAR point cloud or visual image, and constructs loop constraints. When the vehicle has path overlap in continuous blind spots, the loop constraints correct the accumulated error and improve positioning consistency. Spatiotemporal consistency integration is achieved by associating four types of factors with state nodes through the topological structure of the factor graph. Spatiotemporal calibration formulas are calculated to address the differences in temporal scales and spatial precision among different factors, thus realizing the spatiotemporal alignment of multi-source observation data. The formulas used are as follows: ; ; In the formula, This represents the time deviation calibration value between the i-th factor and the j-th factor. and Let represent the original timestamps of the i-th factor and the j-th factor, respectively. This represents the preset time calibration coefficient. Indicates the spatial deviation calibration value. and Let R represent the spatial coordinates of the i-th and j-th factors, respectively, and let R represent the attitude rotation matrix. Represents a spatial translation vector; Simultaneously, a probabilistic correlation model between factors and state nodes is established, transforming the noise characteristics of various observations into factor weight parameters to ensure that the factor graph accurately reflects the reliability of multi-source observation data. The multi-source observation data includes vehicle-mounted sensor observation data, collaborative observation data, and environmental observation data. The factor weight allocation formula is calculated using the following formula: ; In the formula, This represents the weight of the k-th class of factors. This represents the noise variance of the observed data corresponding to the k-th factor. This represents the observed value of the observation data corresponding to the k-th factor. Let x represent the association function between the k-th type of factor and the state node, and let x represent the state node vector. State nodes are used as variable nodes in the factor graph, and four types of factors are used as constraint factors in the factor graph. By combining spatiotemporal calibration relationships and weight allocation results, the topological connection and spatiotemporal association between nodes and factors are completed, and finally a complete spatiotemporally consistent collaborative factor graph is generated and output to the collaborative constraint optimization module.

[0018] Example 4, see Figure 1 This embodiment is based on the above embodiment. The cooperative constraint optimization module is used to solve the problems of insufficient system observability and rapid divergence of inertial navigation errors under GNSS blind zones. Through dynamic cooperative node selection and adaptive constraint weight adjustment, it enhances the state predictability of the system under weak observation conditions, optimizes the cooperative constraint effect, effectively suppresses the cumulative error of inertial navigation, and ensures positioning accuracy under continuous blind zones. It includes the following: A collaborative node is defined as an entity that provides observation information to the current vehicle, participates in collaborative positioning constraints, and is dynamically evaluated and selected in continuous GNSS blind zone scenarios. Specifically, it includes two types: First, vehicle-mounted collaborative nodes, which refer to other vehicles around the current vehicle. Each vehicle corresponds to a set of independent state nodes (pose, velocity, IMU offset) and provides the current vehicle with collaborative observation data such as relative ranging and relative attitude through V2V communication. Second, roadside collaborative nodes, which refer to roadside units deployed around the road. They only serve as observation sources to provide collaborative observation data and transmit observation information such as relative position to the current vehicle through V2I communication. To address the issues of insufficient system observation information and inconsistent quality of collaborative nodes in GNSS blind zones, a collaborative node observation contribution evaluation model is constructed. This model dynamically evaluates the contribution of observation information from surrounding vehicles and roadside units to the current vehicle's positioning. Evaluation indicators include the accuracy of observation data (such as relative ranging error and attitude measurement error), data update frequency, link stability, and the relative position of collaborative nodes to the current vehicle (prioritizing collaborative nodes with closer distance and better observation angles). The optimal collaborative node selection is based on the contribution evaluation results. An optimal collaborative node screening strategy is implemented to eliminate collaborative nodes with low contribution and unreliable observations, and select collaborative nodes with high contribution scores to participate in the current vehicle state estimation. Collaborative nodes are quickly clustered and screened using a clustering algorithm. Combined with the dynamic characteristics of high-speed vehicle movement, the computational complexity is significantly reduced and the real-time performance of collaborative node selection is improved while ensuring screening accuracy. A dynamic node update mechanism is set up to re-evaluate the contribution of collaborative nodes every preset period (500ms). The set of nodes participating in collaboration is dynamically adjusted according to changes in vehicle motion state and communication environment to ensure the effectiveness and real-time performance of collaborative constraints, avoid observation noise introduced by invalid collaborative nodes, and finally obtain the optimal collaborative nodes after screening. Adaptive confidence weight optimization addresses the reliability differences of various observation constraints by introducing an adaptive confidence weight mechanism. This mechanism dynamically assigns weights to four types of factors (IMU pre-integration factor, cooperative relative constraint factor, vehicle-mounted odometer factor, and environmental closed-loop detection factor). This dynamic weight allocation is adaptively adjusted based on the real-time quality of the observation data (e.g., lidar point cloud matching accuracy, noise level of cooperative observation data, and environmental complexity). The observation reliability is quantified into a confidence coefficient within the range of 0 to 1, and this coefficient directly represents the confidence level of each factor. The confidence coefficient is then mapped to the optimized weight of the corresponding factor, forming an adaptive dynamic weight. For example, when blind zone occlusion is severe or local motion constraints are unreliable, the weight of the cooperative relative constraint factor is increased; when the observation quality of cooperative nodes deteriorates, the weights of the IMU pre-integration factor and the environmental closed-loop detection factor are increased. Through weight optimization, the system's adaptability to different observation conditions is enhanced. Observability enhancement optimization, where observability represents the ability to uniquely determine the state of each vehicle through autonomous observation and vehicle-to-vehicle cooperative observation in a vehicle cooperative localization system, constructs an observability enhancement cooperative constraint system based on the selected optimal cooperative nodes and adaptive dynamic weights, and calculates the quantitative formula for the system observability index, as follows: ; ; In the formula, O represents the system observability index, with a value range of [0, 1]. The closer O is to 1, the stronger the system observability. Let represent the observation Jacobian matrix, z represent the multi-source observation vector, x represent the state node vector, n represent the state node dimension, rank represent the rank of the matrix, and T represent the transpose of the vector. Based on the system observability index, the vehicle state nodes are iteratively optimized using a factor graph optimization algorithm to improve the system's state predictability under weak observation conditions. The iterative optimization objective function is calculated to achieve observability-driven constraint optimization, and the formula used is as follows: ; In the formula, M represents the iterative optimization objective function, and K represents the total number of factors. This represents the observability penalty coefficient. This represents the initial estimated value of the state node. The objective function dynamically introduces a penalty term through the observability index to strengthen the initial state constraint and improve the observability of the system in weak observation scenarios (where O is small). To address the issue of accumulated inertial navigation errors, the focus is on strengthening the corrective effect of cooperative constraints and lapsing constraints on IMU offset. Through iterative optimization, the positioning drift caused by IMU angular velocity and acceleration offsets is gradually suppressed, ensuring that the vehicle positioning accuracy remains within the preset range in continuous GNSS blind zones, thus obtaining continuous positioning results that meet the preset accuracy.

[0019] By performing the above operations, using observability-driven collaborative constraint construction and spatiotemporal consistency collaborative factor graph modeling, high observability and stable positioning can still be maintained in scenarios without satellite signals. This achieves unified fusion of multi-source heterogeneous data and solves the technical problems of weak system observability and severe inertial positioning drift caused by GNSS failure during continuous blind zone crossing, as well as the difficulty in unified fusion of spatiotemporal heterogeneous multi-source collaborative observations.

[0020] Example 5, see Figure 1 This embodiment is based on the above embodiment. The adaptive cooperative strategy adjustment module, based on the constructed spatiotemporal consistent cooperative factor graph model, inputs continuous positioning results that meet the preset accuracy, dynamically adjusts the cooperative positioning mode, realizes a smooth transition from "strong cooperation → weak cooperation → autonomous positioning", avoids sudden changes or interruptions in positioning results, and ensures the continuity and robustness of positioning during continuous blind zone crossing, including the following: Real-time communication status determination is achieved by constructing a hierarchical communication status determination model based on communication status information (latency, packet loss rate, bandwidth, etc.) output by the multi-source observation data module. The communication status is divided into three levels—good communication, weakened communication, and communication interruption—based on a comprehensive communication quality score, and the current communication status level is output in real time. The comprehensive communication quality score is calculated using the following formula: ; In the formula, Q represents the overall communication quality score, with a value range of [0, 1]. Indicates transmission delay. B represents the packet loss rate, and B represents the available bandwidth. , , These represent the weighting coefficients, which correspond to the impact weights of latency, packet loss rate, and bandwidth, respectively. When the latency is ≤10ms, the packet loss rate is ≤1%, and the bandwidth is ≥10Mbps, the communication is considered to be good (Q≥0.8). When 10ms < latency ≤ 30ms, 1% < packet loss rate ≤ 5%, and 5Mbps ≤ bandwidth < 10Mbps, it is judged as communication weakening (0.5 ≤ Q < 0.8). If the latency is greater than 30ms, the packet loss rate is greater than 5%, or the link is disconnected and the bandwidth is less than 5Mbps, it is considered a communication interruption (Q < 0.5). The graded collaborative strategy adjustment is based on the current communication status level and continuous positioning results meeting the preset accuracy, combined with the communication quality score Q. The collaborative positioning mode and factor weights are dynamically adjusted, and the adaptive adjustment formula for collaborative weights is calculated as follows: ; ; In the formula, This represents the weight of the collaborative relative constraint factor. This represents the total weight of the onboard autonomous perception constraint factors (onboard odometer factor, IMU pre-integration factor). , These represent the maximum and minimum weight thresholds of the cooperative relative constraint factors, respectively; the communication quality and cooperative weights are dynamically bound through adaptive adjustment of the cooperative weights, ensuring the continuity and accuracy of positioning under different communication conditions; (1) When communication is good, the full cooperative positioning mode is enabled, making full use of multi-vehicle / vehicle-road cooperative observation data, combined with continuous positioning results that meet the preset accuracy, to perform full cooperative constraint optimization, further improve positioning accuracy, and cache the current cooperative information (cooperative node status, constraint weight, local environmental characteristics). (2) When communication is weakened, a smooth degradation strategy is implemented. The weight of the cooperative relative constraint factor is reduced and the weight of the vehicle autonomous perception constraint factor is increased by referring to the historical positioning results that meet the preset accuracy. This achieves a smooth transition from cooperative positioning to autonomous positioning, avoids the positioning results from jumping due to communication weakening, and ensures that the positioning accuracy is always maintained within the preset range. (3) When communication is interrupted, immediately switch to autonomous positioning mode. Based on the continuous positioning results that meet the preset accuracy, rely on the vehicle-mounted multi-sensor (IMU + vision + lidar) for autonomous positioning. At the same time, call the historical collaborative information cached when communication is good to construct predictive collaborative prior constraints. Combine the historical preset accuracy positioning data to suppress the accumulation of errors in the autonomous positioning process and ensure that the positioning accuracy can still be maintained at the preset standard during the communication interruption. The mode switching smooth control, during the switching process of different cooperative modes, takes the continuous positioning results that meet the preset accuracy as the benchmark, introduces a transition smoothing algorithm to fuse and calibrate the vehicle cooperative positioning results before and after the switch, eliminates the positioning jump caused by mode switching, and ensures the continuity of the positioning trajectory. At the same time, it monitors the changes in communication status in real time. When the communication recovers from weakened to good, or from interrupted to weakened / good, the cooperative strategy is gradually adjusted to ensure that the positioning accuracy always meets the preset requirements, realizes the smooth connection of the positioning process, avoids the positioning fluctuation caused by mode switching or communication recovery, and ultimately ensures that the positioning results throughout the process meet the preset accuracy standard, realizing continuous and stable vehicle cooperative positioning output. The transition smoothing algorithm uses continuous positioning results that meet a preset accuracy as a benchmark. At the moment of switching between cooperative modes (strong cooperation, weak cooperation, autonomous positioning), it extracts the historical positioning trajectory data (including state parameters such as pose, velocity, and IMU bias) before the switch and the initial positioning data after the switch. Through weighted fusion, trajectory interpolation, or Kalman smoothing, the two types of data are fused and calibrated to suppress the positioning jump caused by changes in positioning mode (such as sudden changes in cooperative constraint weights or switching of observation sources) during the switching process, so that the positioning trajectories before and after the switch are smoothly connected.

[0021] Example 6, see Figure 1 This embodiment is based on the above embodiment. The collaborative information re-fusion module is used to uniformly fuse and optimize the historical positioning data during the communication interruption and the recovered collaborative information. Through time delay compensation and global optimization, it corrects historical trajectory errors, restores the global consistency of the system state, further improves positioning accuracy and stability, and ensures the overall positioning effect of continuous blind zone crossing. It includes the following: Collaborative data recovery and preprocessing: When the adaptive collaborative strategy module determines that communication has recovered from interruption / weakness to a good state, it first receives delayed collaborative observation data. This collaborative observation data is relative observation data obtained from surrounding vehicle-mounted collaborative nodes via V2V communication, which differs from the autonomously collected IMU, odometer, and environmental closed-loop data, making it more susceptible to communication interruptions and resulting in delayed arrival. Targeted preprocessing is performed on the delayed collaborative observation data, including time alignment and delay compensation. Based on the timestamp record of the communication interruption, the time deviation of the delayed data is corrected, and the delay compensation formula is calculated. The formula used is as follows: ; ; In the formula, Indicates the timestamp after compensation. Indicates the timestamp of the received delayed data. Indicates the total delay. Indicates transmission delay. Indicates data processing latency; By using time delay compensation, the delayed collaborative observation data is precisely time-aligned with the vehicle-mounted autonomous positioning historical data. At the same time, noise filtering and reliability verification are performed on the recovered data to remove bit errors and distorted data caused by communication interruptions, ensuring the quality of the recovered data; thus obtaining preprocessed delayed collaborative observation data. The preprocessed delayed collaborative observation data is reintroduced into the spatiotemporal consistency collaborative factor graph to supplement the collaborative constraint information during the communication interruption. Based on the preprocessed delayed collaborative observation data, a delayed collaborative relative constraint factor is constructed. The topology of the spatiotemporal consistency collaborative factor graph is adjusted according to the time characteristics of this factor, and the delayed collaborative relative constraint factor is associated with the vehicle state nodes of the corresponding time period to ensure that the delayed observation information can effectively play a role in historical state estimation and make up for the lack of collaborative constraints during the communication interruption. Global optimization is performed based on the spatiotemporal consistency coordination factor graph after supplementing the delay coordination relative constraint factor. A global optimization algorithm (using graph optimization or back-end optimization strategy) is executed to uniformly iteratively optimize the historical vehicle state nodes (pose, velocity, IMU bias) during the communication interruption and the current vehicle state. During the optimization process, the delay coordination relative constraint factor corresponding to the preprocessed delay coordination observation data, the historical data of vehicle autonomous positioning during the communication interruption, the environmental closed-loop detection factor, and other multi-source constraint information are fully utilized. At the same time, historical coordination information is introduced to construct predictive coordination prior constraints, and the coordination node state prediction formula is calculated. The formula used is as follows: ; In the formula, This represents the predicted state of the cooperating node at time t. This represents the vehicle's motion model. This represents the state estimate at time t-1. K1 represents the IMU observation input at time t-1, and K1 represents the Kalman gain. Indicates delayed collaborative observations, The observation model is represented by this formula. The state sequence of the cooperating nodes during the communication interruption is predicted time by time. The predicted state is used as the prior expectation to construct the predictive prior constraint factor. The predictive prior constraint factor is added to the spatiotemporal consistency cooperating factor graph to form the predictive cooperating prior constraint, which helps to correct the historical trajectory error. Then, by minimizing the state estimation error (pose estimation error, velocity estimation error, IMU offset estimation error), the positioning drift caused during the communication interruption is corrected, and the error backtracking correction of the historical trajectory is realized to ensure the consistency between the historical trajectory and the current positioning result. After system state calibration and global optimization are completed, the system state is fully calibrated, and the current vehicle pose, speed, IMU offset and other state parameters are updated to restore the global consistency of the system state. This results in a calibrated high-precision vehicle cooperative positioning result and a globally consistent positioning trajectory, ensuring that the system can quickly adapt to cooperative positioning scenarios after communication is restored, and providing high-precision and continuous positioning support for continuous blind zone crossing.

[0022] By performing the above operations, using communication quality-driven adaptive cooperative switching and predictive prior constraints, and performing latency-compensated re-fusion, autonomous positioning without drift is achieved when communication is interrupted, and global trajectory is uniformly calibrated after communication is restored. This provides high-precision and continuous positioning support for continuous blind zone crossings, and solves the technical problems of insufficient positioning continuity and robustness caused by unstable V2X communication leading to cooperative positioning failure and trajectory breakage in continuous blind zone scenarios with communication obstruction and network interruption, and the inability to backtrack and correct historical errors after communication is restored.

[0023] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0024] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention.

[0025] The present invention and its embodiments have been described above. This description is not restrictive, and the accompanying drawings are only one embodiment of the present invention; the actual structure is not limited thereto. In conclusion, if those skilled in the art are inspired by this description and design similar structures and embodiments without departing from the spirit of the invention, such designs should fall within the protection scope of the present invention.

Claims

1. A vehicle cooperative positioning system for traversing continuous blind spots, characterized in that: The system includes a module for acquiring multi-source observation data, a module for constructing a spatiotemporal consistency and synergy factor graph, a synergy constraint optimization module, an adaptive synergy strategy adjustment module, and a synergy information re-fusion module; The module for acquiring multi-source observation data specifically involves collecting multi-source observation data and performing data preprocessing to obtain preprocessed multi-source observation data. The module for constructing a spatiotemporally consistent collaborative factor graph specifically involves constructing a unified state node system and four types of factors based on preprocessed multi-source observation data, performing spatiotemporal calibration and factor weight allocation, and obtaining a spatiotemporally consistent collaborative factor graph. The collaborative constraint optimization module specifically evaluates the contribution of collaborative node observations, selects the optimal collaborative node, optimizes the dynamic weight of factors, quantifies the system observability index, and iteratively optimizes the vehicle status node to obtain continuous positioning results that meet the preset accuracy. The adaptive collaborative strategy adjustment module specifically constructs a communication state hierarchical judgment model, performs adaptive adjustment of collaborative weights based on the current communication state level and continuous positioning results that meet the preset accuracy, and introduces a transition smoothing algorithm to achieve smooth control of mode switching. Specifically, when the adaptive collaborative strategy adjustment module determines that the communication state has recovered to a good state, the delayed collaborative observation data is preprocessed and reintroduced into the spatiotemporal consistency collaborative factor graph for global optimization. Historical collaborative information is introduced to construct predictive collaborative prior constraints, and then the system state is calibrated to obtain the calibrated high-precision vehicle collaborative positioning result.

2. The vehicle cooperative positioning system for traversing continuous blind spots according to claim 1, characterized in that: The module for constructing a spatiotemporally consistent collaborative factor graph includes the following: Based on the preprocessed multi-source observation data, a unified state node system is constructed; each vehicle corresponds to a set of independent state nodes. Based on a unified state node system and the constraint characteristics of multi-source observation data, four types of factors are constructed, all of which directly affect the corresponding state nodes. The four types of factors include IMU pre-integration factor, cooperative relative constraint factor, vehicle odometer factor, and environmental closed-loop detection factor. Spatiotemporal consistency integration is achieved by associating four types of factors with state nodes through the topological structure of the factor graph. Spatiotemporal calibration formulas are calculated for the differences in time scale and spatial precision of different factors to achieve spatiotemporal alignment of multi-source observation data. At the same time, a probabilistic association model between factors and state nodes is established to transform the noise characteristics of various observations into the weight parameters of factors and calculate the factor weight allocation formula. By using state nodes as variable nodes in the factor graph and four types of factors as constraint factors, and combining spatiotemporal calibration relationships and weight allocation results, the topological connection and spatiotemporal association between nodes and factors are completed, and finally a complete spatiotemporally consistent collaborative factor graph is generated.

3. The vehicle cooperative positioning system for traversing continuous blind spots according to claim 1, characterized in that: The collaborative constraint optimization module includes the following: Define a collaborative node, which refers to the entity that provides observation information for the current vehicle, participates in collaborative positioning constraints, and is dynamically evaluated and screened in continuous GNSS blind zone scenarios. Specifically, it includes vehicle-mounted collaborative nodes and roadside collaborative nodes. The contribution of collaborative node observations is evaluated by constructing a collaborative node observation contribution evaluation model to dynamically assess the contribution of observation information from surrounding vehicles and roadside units to the current vehicle's positioning. Evaluation indicators include the accuracy of observation data, data update frequency, link stability, and the relative position of collaborative nodes and the current vehicle. The optimal collaborative node selection is based on the contribution evaluation results. An optimal collaborative node screening strategy is implemented, and collaborative nodes are quickly clustered and screened according to a clustering algorithm. A dynamic node update mechanism is set up to re-evaluate the contribution of collaborative nodes every preset period. The set of nodes participating in collaboration is dynamically adjusted according to changes in vehicle movement status and communication environment, and finally the optimal collaborative nodes are obtained after screening. Adaptive confidence weight optimization introduces an adaptive confidence weight mechanism to dynamically allocate weights to four types of factors, taking into account the reliability differences of different observation constraints. The dynamic weight allocation is adaptively adjusted based on the real-time quality of the observation data to form adaptive dynamic weights. Observability enhancement optimization: Based on the selected optimal collaborative nodes and adaptive dynamic weights, an observability-enhanced collaborative constraint system is constructed to quantify the observability index of the system. Based on the system observability index, the vehicle state nodes are iteratively optimized using the factor graph optimization algorithm, and the iterative optimization objective function is calculated. Through iterative optimization, the positioning drift caused by IMU angular velocity and acceleration offset is gradually suppressed, ensuring that the vehicle positioning accuracy remains within the preset range in continuous GNSS blind zones, and obtaining continuous positioning results that meet the preset accuracy.

4. A vehicle cooperative positioning system for traversing continuous blind spots according to claim 1, characterized in that: The adaptive collaborative strategy adjustment module includes the following: Real-time communication status determination: Based on the communication status information output by the multi-source observation data module, a communication status classification determination model is constructed. According to the comprehensive communication quality score, the communication status is divided into three levels: good communication, weak communication, and communication interruption. The current communication status level is output in real time. The graded collaborative strategy is adjusted based on the current communication status level and the continuous positioning results that meet the preset accuracy. The collaborative positioning mode and factor weights are dynamically adjusted, and the communication quality and collaborative weights are dynamically bound through adaptive adjustment of collaborative weights. Mode switching smooth control, during the switching process of different cooperative modes, takes the continuous positioning results that meet the preset accuracy as the benchmark, introduces a transition smoothing algorithm, and performs fusion calibration on the vehicle cooperative positioning results before and after the switch.

5. A vehicle cooperative positioning system for traversing continuous blind spots according to claim 1, characterized in that: The collaborative information re-fusion module includes the following: Collaborative data recovery and preprocessing: When the adaptive collaborative strategy adjustment module determines that the communication status has recovered to a good state, it first receives the delayed collaborative observation data. The collaborative observation data represents the relative observation data obtained from surrounding vehicle-mounted collaborative nodes through V2V communication in the multi-source observation data. Targeted preprocessing is performed on the delayed collaborative observation data, including time alignment and delay compensation. Based on the timestamp record of the communication interruption, the time deviation of the delayed data is corrected, and the delay compensation formula is calculated. By using time delay compensation, the delayed collaborative observation data is precisely time-aligned with the historical data of vehicle autonomous positioning. At the same time, noise filtering and reliability verification are performed on the recovered data to obtain preprocessed delayed collaborative observation data. The preprocessed delayed collaborative observation data is reintroduced into the spatiotemporal consistency collaborative factor graph to supplement the collaborative constraint information during the communication interruption period. Based on the preprocessed delayed collaborative observation data, a delayed collaborative relative constraint factor is constructed. The topology of the spatiotemporal consistency collaborative factor graph is adjusted according to the time characteristics of this factor, and the delayed collaborative relative constraint factor is associated with the vehicle status nodes of the corresponding time period. Global optimization is performed based on the spatiotemporal consistency coordination factor graph after supplementing the delay coordination relative constraint factor. The global optimization algorithm is executed to uniformly iterate and optimize the historical vehicle state nodes and the current vehicle state during the communication interruption. During the optimization process, multi-source constraint information is fully utilized, and predictive coordination prior constraints are introduced to predict the state of coordination nodes. The state sequence of coordination nodes during the communication interruption is predicted time by time. The predicted state is used as the prior expectation to construct predictive prior constraint factors. The predictive prior constraint factors are added to the spatiotemporal consistency coordination factor graph to form predictive coordination prior constraints, which help to correct historical trajectory errors. Then, by minimizing the state estimation error, which includes pose estimation error, velocity estimation error, and IMU bias estimation error, the positioning drift generated during the communication interruption is corrected, realizing the error backtracking correction of the historical trajectory. After system state calibration and global optimization are completed, the system state is fully calibrated, the current state parameters of the vehicle are updated, the global consistency of the system state is restored, and the calibrated high-precision vehicle cooperative positioning results and globally consistent positioning trajectory are obtained.

6. A vehicle cooperative positioning system for traversing continuous blind spots according to claim 1, characterized in that: The module for acquiring multi-source observation data includes the following: Real-time acquisition of multi-source observation data, including data from vehicle-mounted sensors, collaborative observation data, and communication status information; Obtain the raw multi-source observation data; Data preprocessing involves cleaning, synchronizing, and unifying the coordinates of the collected raw multi-source observation data to obtain preprocessed multi-source observation data.

Images