An asynchronous multi-thread filtering positioning method for a vehicle and related device

Abstract

The application discloses an asynchronous multi-thread filtering positioning method for a vehicle and related equipment, and the method comprises the following steps: acquiring observation data of a main sensor and an auxiliary sensor; synchronizing auxiliary observation data based on a main sensor time axis, constructing a synchronous measurement package, and maintaining a snapshot cache area arranged along the time axis; when current main observation data is received, triggering a real-time filtering thread, determining a previous time positioning state based on a first snapshot in the snapshot cache area, and predicting a current time positioning state; when auxiliary observation data is received, triggering a delay filtering thread, predicting a corrected current time positioning state based on a second snapshot and target observation data obtained from the synchronous measurement package; and maintaining consistency of states of the two filtering threads by updating a snapshot reference shared by the two filtering threads. The application realizes decoupling of high-frequency prediction and delay observation update, guarantees time consistency of state estimation, and improves positioning accuracy, real-time performance and robustness in a complex scene.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, specifically to the field of multi-sensor fusion positioning technology, and particularly to an asynchronous multi-threaded filtering positioning method and related equipment for vehicles. Background Technology

[0002] In multi-sensor fusion positioning scenarios, it is typically necessary to fuse data from various heterogeneous sensors such as inertial measurement units, satellite navigation systems, and lidar to achieve high-precision positioning. These sensors suffer from issues such as large differences in sampling frequencies, inconsistent data transmission delays, and clock asynchrony. Existing filtering positioning architectures based on single-threaded sequential execution have significant shortcomings when processing such asynchronous data. First, when low-frequency, high-latency auxiliary observation data arrives, the system state has already been predicted in multiple steps by the high-frequency main sensor. Direct fusion will result in positioning errors due to timestamp mismatch and state mismatch. If a blocking state backtracking and repropagation method is used to process delayed data, the real-time positioning process will be suspended, and the computational overhead will increase significantly with the backtracking length, severely reducing the system's real-time performance.

[0003] Secondly, the high-frequency main sensor prediction and the low-frequency, high-computation auxiliary observation update share the same computing thread. The auxiliary observation processing will block the main sensor data flow, causing the system positioning output frequency to be limited by the slowest observation module, which cannot meet the high real-time requirements of scenarios such as autonomous driving and real-time navigation.

[0004] Third, if a dual-threaded architecture is used to handle prediction and update separately, it is difficult to maintain the consistency of the positioning state of the two threads under lock-free conditions, which can easily lead to problems such as state jumps and discontinuous trajectories.

[0005] Therefore, how to decouple high-frequency prediction from high-latency observation updates, and effectively integrate asynchronous multi-source data while ensuring real-time performance, thereby improving positioning accuracy and system robustness, is a technical problem that urgently needs to be solved in this field. Summary of the Invention

[0006] This application provides an asynchronous multi-threaded filtering positioning method and device for vehicles, which effectively integrates asynchronous multi-source data while ensuring real-time performance, thereby improving positioning accuracy and system robustness.

[0007] The technical solution is as follows: Firstly, an asynchronous multi-threaded filtering localization method for vehicles is provided, comprising: Acquire the main observation data from the main sensor and the auxiliary observation data from at least one auxiliary sensor; Based on the time axis of the main sensor, all auxiliary observation data are synchronized in time, a synchronization measurement package is constructed, and a snapshot cache area arranged along the time axis is maintained; When the current master observation data is received, the real-time filtering thread is triggered to obtain the first snapshot with the latest timestamp before the current moment from the snapshot buffer. Based on the first snapshot, the positioning status at the previous moment is determined, and based on the positioning status at the previous moment and the current master observation data, the positioning status at the current moment is predicted and output. When the auxiliary observation data is received, a delay filtering thread is triggered to retrieve the second snapshot with the latest timestamp before the timestamp of the auxiliary observation data in the snapshot buffer, and to obtain the target observation data from the synchronization measurement packet. Using the target observation data, the corrected positioning status at the current moment is predicted; wherein, the target observation data is all synchronization measurement packets between the timestamp of the second snapshot and the current moment. By updating the snapshot reference shared by the two filtering threads, the real-time filtering thread can obtain the corrected positioning state at the current moment obtained by the delayed filtering thread, thereby maintaining the consistency of the states of the two filtering threads.

[0008] In one possible implementation, all auxiliary observation data are time-synchronized based on the time axis of the main sensor to construct a synchronization measurement package, including: The main observation data and each auxiliary observation data are respectively converted into their respective standardized data structures; Based on the time axis of the main sensor, a preset linear time model and least squares estimation are used to perform clock ratio compensation and fixed delay compensation on the original timestamp of each auxiliary observation data to obtain the synchronization timestamp of each auxiliary observation data. Based on the nearest neighbor principle, each synchronization timestamp is assigned to the observation time of the nearest master sensor; All sub-auxiliary observation data allocated to the observation time of the main sensor are bound to the sub-main observation data of the observation time of the main sensor to form the synchronous measurement package; Maintaining snapshot caches arranged along the time axis includes: At each observation time of the main sensor, a snapshot is generated, including the timestamp, nominal state, error state, and error covariance matrix of each observation time; Snapshots of each observation moment are stored in the snapshot cache area in order of timestamp.

[0009] In one possible implementation, the positioning state at the previous moment is determined based on the first snapshot, and the positioning state at the current moment is predicted and output based on the positioning state at the previous moment and the current master observation data, including: When the timestamp of the first snapshot is equal to the timestamp of the previous moment, the positioning status in the first snapshot is used as the positioning status of the previous moment. When the timestamp of the first snapshot is earlier than the timestamp of the previous moment, the snapshot playback mechanism is triggered: all synchronous measurement packets in the snapshot buffer from the timestamp of the first snapshot to the timestamp of the previous moment are traversed, and error state Kalman filter prediction is performed on the main sensor observation data in each synchronous measurement packet in the order of timestamps. The state in the first snapshot is then pushed back to the previous moment to obtain the positioning state of the previous moment. Based on the positioning state of the previous moment, and combined with the current master observation data, an error state Kalman filter prediction is performed to obtain the prior positioning state of the current moment, which is then output as the current positioning state.

[0010] In one possible implementation, predicting the corrected positioning state at the current moment using the target observation data includes: Each synchronous measurement packet in the target observation data is processed sequentially according to the timestamp order, and the estimated state at the current time obtained after recursive processing of all synchronous measurement packets is used as the corrected positioning state at the current time. In the process of processing each synchronous measurement package in the target observation data, if the current synchronous measurement package contains auxiliary observation data, then an error state Kalman filter update is performed based on the auxiliary observation data; if the current synchronous measurement package does not contain auxiliary observation data, then an error state Kalman filter prediction is performed based on the master observation data in the current synchronous measurement package.

[0011] In one possible implementation, the method further includes: When the delay filtering thread processes the synchronous measurement package, it calculates a comprehensive quality index for each auxiliary observation data in the synchronous measurement package; wherein, the comprehensive quality index is calculated by a preset quality evaluation function based on at least one parameter among the signal strength, geometric distribution accuracy factor, point cloud density, and residual size of the auxiliary observation data; Determine whether the comprehensive quality index meets the preset adaptive threshold; If the conditions are met, the auxiliary observation data is retained for use in the error state Kalman filter update. If the conditions are not met, the auxiliary observation data shall be discarded.

[0012] In one possible implementation, the method further includes: When the comprehensive quality index of a first preset number of consecutive auxiliary observation data does not meet the adaptive threshold, the process of the delayed filtering thread predicting the corrected positioning status at the current moment is paused, and the positioning status at the current moment is continued to be predicted and output only by the real-time filtering thread. When the comprehensive quality index of a second consecutive preset number of auxiliary observation data is detected to meet the adaptive threshold, the delay filtering thread is triggered again to perform the process of predicting the corrected positioning state at the current moment.

[0013] In one possible implementation, by updating the snapshot reference shared by the two filtering threads, the real-time filtering thread is able to obtain the corrected positioning state at the current moment obtained by the delayed filtering thread, including: An atomic replacement method is used to update the snapshot reference output by the real-time filtering thread to point to the snapshot output by the delayed filtering thread, so that subsequent prediction operations of the real-time filtering thread are executed based on the corrected positioning state in the snapshot output by the delayed filtering thread.

[0014] In one possible implementation, the method further includes: The current positioning status is smoothed and filtered to obtain the final positioning trajectory status at the current moment.

[0015] Secondly, an asynchronous multi-threaded filtering positioning device for vehicles is provided, comprising: The acquisition module is used to acquire the main observation data from the main sensor mounted on the vehicle and the auxiliary observation data from at least one auxiliary sensor. The synchronization module is used to synchronize all auxiliary observation data in time based on the time axis of the main sensor, construct a synchronization measurement package, and maintain a snapshot cache area arranged along the time axis; The first prediction module is used to trigger a real-time filtering thread when it receives the current master observation data at the current moment, obtain the first snapshot with the latest timestamp before the current moment from the snapshot cache, determine the positioning status at the previous moment based on the first snapshot, and predict and output the positioning status at the current moment based on the positioning status at the previous moment and the current master observation data. The second prediction module is used to, when receiving the auxiliary observation data, trigger a delay filtering thread, retrieve the second snapshot with the latest timestamp before the timestamp of the auxiliary observation data in the snapshot cache, and obtain target observation data from the synchronization measurement packet, and use the target observation data to predict the corrected positioning status at the current time; wherein, the target observation data is all synchronization measurement packets between the timestamp of the second snapshot and the current time; A correction module is used to update a snapshot reference shared by two filtering threads, enabling the real-time filtering thread to obtain the corrected positioning state at the current moment obtained by the delayed filtering thread, thereby maintaining the consistency of the states of the two filtering threads. In a third aspect, an electronic device is provided, comprising: At least one processor; and A memory communicatively connected to the at least one processor; wherein, The memory stores instructions executable by the at least one processor, which, when executed by the at least one processor, enables the at least one processor to perform the asynchronous multi-threaded filtering localization method for a vehicle according to any of the preceding claims.

[0016] Fourthly, a vehicle is provided that includes the electronic equipment described above.

[0017] Fifthly, a computer-readable storage medium is provided, wherein at least one instruction is stored therein, the at least one instruction being loaded and executed by a processor to implement the aspects described above and any possible implementation thereof.

[0018] In a sixth aspect, a computer program product is provided, comprising a computer program, wherein the computer program, when executed by a processor, implements the aspects and any possible implementations described above.

[0019] The beneficial effects of the technical solution provided in this application include at least the following: As can be seen from the above technical solution, the embodiments of this application complete the time synchronization of all auxiliary observation data based on the time axis of the main sensor, construct a synchronous measurement package and maintain a snapshot buffer area arranged along the time axis; adopt a dual-thread architecture with parallel real-time filtering thread and delayed filtering thread, the real-time filtering thread outputs the positioning status at high frequency based on historical snapshots and current main observation data, and the delayed filtering thread completes the backtracking correction of delayed observations based on historical snapshots and synchronous measurement packages of the corresponding time period; by atomically updating the snapshot reference shared by the two threads, the consistency of the positioning status of the two threads is maintained under lock-free conditions. In this way, the high-frequency positioning prediction and delayed observation update are decoupled, and the auxiliary observation data with arbitrary delay can be processed without blocking the real-time output, while ensuring the time consistency of the positioning status and the continuity of the trajectory, effectively taking into account the accuracy, real-time performance and system stability of the fused positioning.

[0020] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of this application, nor is it intended to limit the scope of this application. Other features of this application will become readily apparent from the following description. Attached Figure Description

[0021] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0022] Figure 1 This is a flowchart illustrating an embodiment of an asynchronous multi-threaded filtering positioning method for vehicles provided in this application. Figure 2 This is a structural block diagram of an asynchronous multi-threaded filtering positioning device for a vehicle provided in one embodiment of this application; Figure 3 This is a schematic block diagram of an example electronic device according to an embodiment of this application. Detailed Implementation

[0023] The following description, in conjunction with the accompanying drawings, illustrates exemplary embodiments of this application, including various details to aid understanding. These embodiments should be considered merely exemplary. Therefore, those skilled in the art will recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of this application. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0024] Obviously, the described embodiments are only some, not all, of the embodiments in this application. All other embodiments obtained by those skilled in the art based on the embodiments in this application without inventive effort are within the scope of protection of this application.

[0025] Furthermore, the term "and / or" in this article is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, or B existing alone. Additionally, the character " / " in this article generally indicates that the preceding and following related objects have an "or" relationship.

[0026] Figure 1 This is a flowchart illustrating an embodiment of the asynchronous multi-threaded filtering and positioning method for vehicles provided in this application, as shown below. Figure 1 As shown, the main steps of the asynchronous multi-threaded filtering localization method for vehicles in this embodiment may include: 101. Acquire the main observation data from the main sensor mounted on the vehicle and the auxiliary observation data from at least one auxiliary sensor; High-frequency acceleration and angular velocity data can be acquired from the inertial measurement unit (as the main sensor) as primary observation data, while position, velocity, or point cloud data can be acquired from the global navigation satellite system receiver and lidar (as auxiliary sensors) as auxiliary observation data. The main sensor can be selected according to actual needs.

[0027] 102. Based on the time axis of the main sensor, synchronize all auxiliary observation data in time, construct a synchronization measurement package, and maintain a snapshot buffer area arranged along the time axis; In a specific implementation, the process of constructing a synchronization measurement package may include: The main observation data and each auxiliary observation data are respectively converted into their respective standardized data structures; Based on the time axis of the main sensor, a preset linear time model and least squares estimation are used to perform clock ratio compensation and fixed delay compensation on the original timestamp of each auxiliary observation data to obtain the synchronization timestamp of each auxiliary observation data. Based on the nearest neighbor principle, each synchronization timestamp is assigned to the observation time of the nearest master sensor; All sub-auxiliary observation data allocated to the observation time of the main sensor are bound to the sub-main observation data of the observation time of the main sensor to form the synchronous measurement package.

[0028] In detail, a standardized data structure can be created for each frame of data received, which includes at least a data source identifier (to distinguish IMU, GNSS, LiDAR, etc.), an observation vector, and a raw timestamp.

[0029] Time synchronization of all non-IMU auxiliary observation data can be performed using the IMU's timeline as a reference. Specifically, for an auxiliary sensor, it is assumed that there is a linear relationship between its clock and the main sensor's clock, including a clock rate scaling factor and a fixed time offset. Multiple pairs of data from the auxiliary sensor over a recent period can be maintained, each pair containing the sensor's original timestamp and its nearest IMU timestamp. Using a least-squares estimation algorithm, these data pairs are used to solve for the optimal clock scaling factor and fixed delay compensation value. Then, for each newly arriving auxiliary observation data point, its original timestamp is converted to a synchronization timestamp consistent with the IMU's timeline using the solved compensation factors and compensation values.

[0030] After obtaining the synchronization timestamp, the timestamp sequence of the IMU data stream is traversed to find the IMU observation time closest to the synchronization timestamp on the timeline. This auxiliary observation data is then assigned to this IMU observation time. At each IMU observation time, the IMU observation data for that time itself is packaged with all the auxiliary observation data assigned to that time to form a synchronization measurement packet.

[0031] In a specific implementation, maintaining a snapshot cache arranged along the time axis includes: generating a snapshot at each observation time of the main sensor, including the timestamp, nominal state, error state, and error covariance matrix of each observation time; and storing the snapshot of each observation time into the snapshot cache in the order of the timestamps.

[0032] In detail, when maintaining the snapshot buffer, a snapshot can be generated at each IMU observation time. This snapshot is essentially a record of the complete state at that moment, including: the timestamp of that moment, the nominal system state used for subsequent nonlinearization processing, the error state used for linearization error processing, and the error covariance matrix characterizing the uncertainty. All generated snapshots are stored in a pre-allocated buffer in chronological order of their timestamps, so that subsequent real-time threads and delayed threads can quickly backtrack and retrieve them.

[0033] 103. When the current master observation data at the current moment is received, the real-time filtering thread is triggered to obtain the first snapshot with the latest timestamp before the current moment from the snapshot buffer. Based on the first snapshot, the positioning status at the previous moment is determined, and based on the positioning status at the previous moment and the current master observation data, the positioning status at the current moment is predicted and output. In a specific implementation, when a new frame of IMU data is received at the current moment, a real-time filtering thread can be triggered. This thread first searches the snapshot buffer for the latest snapshot whose timestamp is no later than the current moment; this is called the first snapshot. When the timestamp of the first snapshot equals the timestamp of the previous moment, the positioning state in the first snapshot is used as the positioning state of the previous moment. When the timestamp of the first snapshot is earlier than the timestamp of the previous moment, a snapshot playback mechanism is triggered: it iterates through all synchronous measurement packets in the snapshot buffer between the timestamp of the first snapshot and the timestamp of the previous moment, sequentially performing error state Kalman filter prediction on the master sensor observation data in each synchronous measurement packet according to the timestamp order, recursively pushing the state in the first snapshot to the previous moment to obtain the positioning state of the previous moment; based on the positioning state of the previous moment, combined with the current master observation data, error state Kalman filter prediction is performed to obtain the prior positioning state of the current moment, which is then output as the current positioning state.

[0034] In detail, when the real-time filtering thread receives new IMU data, its core task is to accurately recursively calculate the system state from the previously known state to the current moment. The real-time filtering thread first obtains the first snapshot, which is the latest snapshot in the snapshot cache with a timestamp no later than the current moment.

[0035] Ideally, the snapshot buffer is complete, and the time of the first snapshot is exactly equal to the previous time. In this case, the real-time filtering thread can directly use the position, velocity, attitude, and other state information stored in the first snapshot as the system's positioning state at the previous time.

[0036] In actual operation, because the delay filtering thread may be correcting and updating the shared snapshot reference in the background, the first snapshot time obtained by the real-time thread may be earlier than the previous time. In this case, the system will activate the snapshot playback mechanism. This mechanism first finds all synchronous measurement packets with timestamps between the first snapshot time and the previous time from the snapshot buffer. Then, the delay filtering thread starts from the state recorded in the first snapshot and processes these synchronous measurement packets sequentially according to time. When processing each packet, since these packets are only used for state recursion and do not perform external observation updates, only the IMU observation data in the packet is extracted, and the error state Kalman filtering prediction step is performed. This process will gradually recursively advance the system state from the first snapshot time, through multiple intermediate IMU observation times, and finally accurately recursively advance to the previous time, thus obtaining the positioning state at the previous time.

[0037] Regardless of how the accurate positioning state of the previous moment is obtained, the real-time filtering thread will ultimately use this state as the initial value and, in conjunction with the newly arrived IMU observation data, perform an error state Kalman filter prediction step. The result of this prediction, i.e., the prior positioning state of the current moment, will be output as the system's real-time positioning result for use by downstream modules (such as path planning and vehicle control).

[0038] This embodiment solves the problem of state asynchrony caused by background correction by introducing a snapshot playback mechanism into the real-time filtering thread, ensuring that the real-time thread can always make predictions from an accurate historical state. This mechanism does not require waiting for or blocking delayed threads, thus guaranteeing the stability of the real-time output frequency. At the same time, by accurately recursively extrapolating to the previous moment rather than directly predicting from an earlier snapshot, the time span of a single prediction is reduced, improving the accuracy of state prediction.

[0039] 104. When the auxiliary observation data is received, a delay filtering thread is triggered to retrieve the second snapshot with the latest timestamp before the timestamp of the auxiliary observation data in the snapshot buffer, and target observation data obtained from the synchronization measurement packet is used to predict the corrected positioning status at the current moment. When delayed auxiliary observation data (e.g., a GNSS signal acquired 200 milliseconds ago) is received, a delay filtering thread is triggered. This thread first retrieves the latest snapshot in the snapshot buffer, whose timestamp is no later than the timestamp of the auxiliary observation data; this is called the second snapshot. Then, it retrieves all synchronization measurement packets from the timestamp of the second snapshot to the current system time from the synchronization measurement packet buffer; these packets constitute the target observation data. Starting from the state of the second snapshot, the delay filtering thread processes each synchronization measurement packet in the target observation data sequentially according to its timestamp, progressively recursively pushing the state back to the current time, ultimately obtaining the corrected positioning state for the current time. During this recursive process, if a synchronization measurement packet contains auxiliary observation data (such as GNSS or LiDAR data), an error state Kalman filter update step is performed to correct the state; if it only contains IMU data, only a prediction step is performed.

[0040] In a specific implementation, predicting the corrected positioning state at the current moment using the target observation data includes: Each synchronous measurement packet in the target observation data is processed sequentially according to the timestamp order, and the estimated state at the current time obtained after recursive processing of all synchronous measurement packets is used as the corrected positioning state at the current time. In the process of processing each synchronous measurement package in the target observation data, if the current synchronous measurement package contains auxiliary observation data, then an error state Kalman filter update is performed based on the auxiliary observation data; if the current synchronous measurement package does not contain auxiliary observation data, then an error state Kalman filter prediction is performed based on the master observation data in the current synchronous measurement package.

[0041] In detail, after the delay filtering thread is triggered, it has obtained the second snapshot for backtracking, as well as all synchronization measurement packets from the timestamp of that snapshot to the current time. These synchronization measurement packets constitute the target observation data. The delay filtering thread then begins to perform recursive calculations to correct the positioning state.

[0042] The specific processing procedure is as follows: The delay filtering thread first extracts the earliest timestamped synchronization measurement packet from the target observation data, and uses the state recorded in the second snapshot as the initial state for recursion. Then, it checks the composition of the synchronization measurement packet. If the synchronization measurement packet only contains the main observation data, i.e., the acceleration and angular velocity data from the inertial measurement unit, and does not contain any observation data from auxiliary sensors (such as global navigation satellites or lidar), the delay filtering thread executes the error state Kalman filtering prediction step. This step uses the inertial measurement unit data within the packet to extrapolate the state from the current time step forward by one step through the state transition function, obtaining the prior state estimate and its covariance matrix for the next time step.

[0043] If the current synchronous measurement packet contains one or more auxiliary observation data (such as the positioning result of a global navigation satellite or the pose constraint obtained by matching a lidar point cloud) in addition to inertial measurement unit (IMU) data, the delay filtering thread executes the error state Kalman filtering update step. Specifically, the delay filtering thread first performs a prediction step using the IMU data within the packet, recursively pushing the state to the time corresponding to that synchronous measurement packet; then, it constructs an observation model using the auxiliary observation data within the packet, calculates the observation residual and Kalman gain, corrects the predicted state, and obtains the posterior state estimate for that time.

[0044] After processing the current synchronous measurement packet, the delay filtering thread processes the next synchronous measurement packet in timestamp order, using the state obtained from processing the previous packet as the initial state of the current packet. This process is repeated, performing the above judgment and processing operations on each synchronous measurement packet in the target observation data. During the processing of each packet, whether an update step is performed depends entirely on whether the packet contains auxiliary observation data, without considering the original arrival delay of the auxiliary observation data. When all synchronous measurement packets have been processed, the final state obtained is the estimated state for the current time after fusing all historical auxiliary observation information. This state is output as the corrected positioning state for the current time.

[0045] This embodiment processes each synchronous measurement packet sequentially according to its timestamp. Within each packet, it dynamically selects between the prediction or update steps of the error state Kalman filter based on whether it contains auxiliary observation data. This achieves the fusion of external observations with time delays and historical state sequences. This processing method ensures that the filtering process strictly follows the chronological order, avoiding time mismatch problems caused by data disorder or delay. This ensures that the fused corrected positioning state maintains consistency and continuity on the timeline, significantly improving the accuracy of multi-sensor fusion positioning.

[0046] 105. By updating the snapshot reference shared by the two filtering threads, the real-time filtering thread can obtain the corrected positioning state at the current moment obtained by the delayed filtering thread, so as to maintain the consistency of the states of the two filtering threads.

[0047] In a specific implementation, to ensure consistency in the output states of two parallel filtering threads, a shared snapshot reference can be used for management. When the real-time filtering thread is working, it references the latest snapshot for prediction and output. When the delayed filtering thread completes a correction calculation and obtains a more accurate corrected positioning state at the current moment, it generates a new snapshot. An atomic substitution method can be used to update the snapshot reference output by the real-time filtering thread to point to the snapshot output by the delayed filtering thread, so that subsequent prediction operations of the real-time filtering thread are performed based on the corrected positioning state in the snapshot output by the delayed filtering thread. In this way, when the real-time filtering thread is triggered again, it will make predictions based on this corrected, more accurate state, thus achieving seamless connection and consistency maintenance of the two thread states.

[0048] In detail, after the delay filtering thread completes a series of complex historical backtracking and observation updates, it calculates a more accurate corrected positioning state for the current moment and encapsulates this new state into a new snapshot. At this point, instead of copying the data of the new snapshot byte-by-byte to the real-time thread's workspace, an efficient atomic replacement operation is used. This operation is atomic, meaning it will not be interrupted by any thread scheduling mechanism, thus ensuring the safety and consistency of the operation. This operation instantly switches the shared snapshot reference from an old address (e.g., pointing to a snapshot previously used by the real-time thread) to a new address, making it directly point to the new snapshot just generated by the delay filtering thread.

[0049] For the real-time filtering thread, the first thing it does when it is triggered by IMU data and begins execution again is to read this shared snapshot reference. Since the reference has been atomically replaced, it no longer reads the old snapshot, but rather a new snapshot that points to the corrected positioning state. Therefore, all subsequent prediction operations of the real-time filtering thread will naturally be performed based on this more accurate latest state, without needing to know what specific corrections have been made in the background.

[0050] This embodiment updates the shared snapshot reference through atomic replacement, providing a lock-free and non-blocking high-efficiency state synchronization mechanism for dual threads. This method avoids the performance overhead and deadlock risk of traditional synchronization methods such as mutexes, allowing the correction results from the background to be instantly transmitted to the high-frequency threads in the foreground, ensuring both state consistency and maintaining the system's high real-time output capability.

[0051] Furthermore, the method may further include: when the delay filtering thread processes the synchronous measurement packet, calculating a comprehensive quality index for each auxiliary observation data in the synchronous measurement packet; wherein the comprehensive quality index is calculated by a preset quality evaluation function based on at least one parameter among the signal strength, geometric distribution accuracy factor, point cloud density, and residual size of the auxiliary observation data; Determine whether the comprehensive quality index meets the preset adaptive threshold; If the conditions are met, the auxiliary observation data is retained for use in the error state Kalman filter update. If the conditions are not met, the auxiliary observation data shall be discarded.

[0052] In detail, when the delay filtering thread processes each synchronous measurement packet in the order of timestamps, it does not directly use the auxiliary observation data for error state Kalman filter update when it encounters a synchronous measurement packet containing auxiliary observation data. Instead, it first starts a dynamic evaluation process of observation quality to determine the reliability of the observation data.

[0053] Specifically, for each auxiliary observation data point in the synchronous measurement package, a pre-built quality evaluation function can be invoked to calculate its comprehensive quality index. This quality evaluation function can comprehensively consider multiple parameters characterizing observation quality. For example, for Global Navigation Satellite System (GNSS) observation data, its signal strength value and satellite geometric distribution accuracy factor can be obtained; for LiDAR observation data, the residual magnitude of point cloud matching and effective point cloud density can be calculated; for other types of auxiliary observation data, corresponding quality parameters can also be selected. The quality evaluation function weights and fuses these parameters according to preset weights, ultimately outputting a quantified comprehensive quality index value, where a higher value indicates better observation quality.

[0054] Meanwhile, an adaptive quality threshold can be dynamically calculated based on the current operating environment. This adaptive threshold is not fixed but can be dynamically adjusted based on the statistical characteristics of the observation quality over a period of time or based on scene identification results. For example, a lower threshold can be set in open environments to make full use of the observation data, while the threshold can be increased in complex signal environments such as urban canyons to filter out potential anomalous observations.

[0055] After obtaining the comprehensive quality index, it can be compared with an adaptive threshold. If the comprehensive quality index is greater than or equal to the adaptive threshold, the auxiliary observation data is deemed to be of acceptable quality and reliable. The system retains this observation data and uses it to perform the subsequent error state Kalman filter update step to correct the current state estimate. Conversely, if the comprehensive quality index is less than the adaptive threshold, the auxiliary observation data is deemed to potentially contain significant noise, multipath effects, or be completely erroneous. This observation data is discarded and not used in this update. In this case, for the current synchronous measurement packet, the delay filtering thread only uses the inertial measurement unit data within the packet to perform the prediction step, without performing an update. For cases where the same synchronous measurement packet contains multiple auxiliary observation data, the above quality assessment is performed on each auxiliary observation data separately, and the selection of each observation is determined independently.

[0056] This embodiment introduces a dynamic evaluation mechanism for observation quality before the update step of the delay filtering thread. This mechanism enables intelligent screening of multi-source auxiliary observation data and utilizes adaptive thresholds for dynamic filtering, effectively eliminating abnormal observation interference caused by complex environments such as GPS signal blockage, multipath effects, and lidar point cloud degradation. This mechanism improves the robustness and positioning accuracy of the fusion positioning system in complex scenarios and avoids contamination and divergence of the filtering results caused by low-quality observations.

[0057] Furthermore, the method may also include: when the comprehensive quality index of a first preset number of consecutive auxiliary observation data does not meet the adaptive threshold, pausing the process of the delayed filtering thread to predict the corrected positioning state at the current moment, and only using the real-time filtering thread to continue predicting and outputting the positioning state at the current moment; when it is detected again that the comprehensive quality index of a second preset number of consecutive auxiliary observation data meets the adaptive threshold, triggering the delayed filtering thread to predict the corrected positioning state at the current moment again.

[0058] In detail, during continuous system operation, auxiliary sensors (such as GNSS) may be in a low-quality state for extended periods, for example, when a vehicle enters a long tunnel. To avoid the delay filtering thread continuously processing this low-quality observation data, wasting computational resources and potentially introducing error correction, an adaptive degradation strategy can be implemented.

[0059] Specifically, a counter can be maintained to count the number of auxiliary observation data points consecutively classified as low-quality (i.e., the overall quality index does not meet the adaptive threshold). When this counter reaches a first preset number (e.g., 10 consecutive times), the system determines that the current environment has rendered the auxiliary observation data unusable for an extended period. At this point, the operating mode can be automatically downgraded. Specifically, the delay filtering thread is paused or suspended from performing the calculation process to correct the positioning status. The background process no longer attempts to use these low-quality GNSS or LiDAR data for backtracking updates. The system's positioning output will be entirely handled by the real-time filtering thread, which relies solely on high-frequency IMU data for pure inertial recursive prediction and continuously outputs the positioning results.

[0060] It can continuously monitor newly arriving auxiliary observation data. When the comprehensive quality index of a second preset number (e.g., three consecutive) of auxiliary observation data is detected to meet the adaptive threshold, it is determined that the auxiliary sensor has recovered to normal operating status. At this time, it will automatically exit the degradation mode and re-trigger the delay filtering thread. The delay filtering thread will perform a complete backtracking update process based on the most recent qualified historical observation data and the second snapshot, quickly correcting the errors accumulated during the pure inertial recursion, so that the system state reconverges.

[0061] This embodiment introduces an automatic degradation and recovery mechanism based on continuous quality statistics. This proactively stops ineffective background calculations to conserve resources in harsh environments where auxiliary observation data is unavailable for extended periods, and maintains continuous positioning output using the high-frequency main sensor, avoiding positioning divergence caused by erroneous fusion observations. When observation conditions recover, it automatically identifies and reactivates background corrections, quickly eliminating accumulated errors from pure inertial recursion. This achieves seamless, adaptive switching across different scenarios, enhancing the fusion positioning system's all-weather, all-scenario capabilities and robustness.

[0062] In some embodiments, the method may further include performing a smoothing filter on the current positioning state to obtain the final positioning trajectory state at the current moment.

[0063] In detail, both the predicted positioning state directly output by the real-time filtering thread and the corrected positioning state output after correction by the delayed filtering thread, as the final output of the fusion positioning system, can undergo a post-processing step to further improve the smoothness of the trajectory. Although the dual-thread mechanism of this application has maintained the consistency of the state through snapshot references and avoided large state jumps, in some extreme cases, such as when the system has just recovered from a long-term pure inertial prediction mode to a mode with external observation updates, the output state sequence may still have slight high-frequency jitter in the time domain.

[0064] To address this issue, a lightweight smoothing filter can be added before the final positioning result. This process maintains historical output states within a short time window. After calculating the current positioning state (whether from a real-time or delayed thread), it is not immediately output as the final result but is instead fed into a smoothing filter. This filter can be a simple moving average filter or a low-pass filter. It combines the current state with the historical states from the previous few time steps within the window to calculate a smoother output value with less high-frequency noise. For example, a weighted moving average method can be used, assigning a higher weight to the current state and decreasing weights to the historical states. The final weighted average is then used as the final positioning trajectory state output for the current moment. This process effectively suppresses high-frequency noise in the state estimation results, making the output trajectory smoother, more continuous, and more consistent with the laws of physics.

[0065] Based on the above, it can be seen that the asynchronous multi-threaded filtering localization method for vehicles in this application mainly includes four stages: Phase 1: Synchronous Preprocessing of Data from Multi-Source Heterogeneous Sensors; Step 1: Asynchronous reception and buffering of multi-source data. First, multiple sensor data streams can be subscribed to separately. Let the j-th observation of the i-th sensor be denoted by equation (1): (1) In the formula, This represents observed values ​​(such as acceleration and angular velocity from an IMU, position and velocity observations from GNSS, and pose constraints from LiDAR). This represents the local timestamp of the sensor. Since different sensors have independent clocks and different sampling frequencies, independent circular buffers can be used to cache data from different types of sensors. Each raw observation is uniformly converted into a standardized data structure, as shown in equation (2): (2) In the formula, The sensor type or identification number (e.g., 0 for IMU, 1 for GNSS, 2 for LiDAR) can be used to distinguish different types or instances of sensors.

[0066] This step allows all types of observation streams to be received independently and stored asynchronously without affecting the system's real-time performance, ensuring that valid historical data can be read at any time during subsequent processing stages.

[0067] Step 2: Data correlation and alignment based on the IMU time axis. Since ESKF uses the IMU as the primary observation source, the IMU observation time... It is defined as the system's main time axis. For all external observations, their original timestamps need to be mapped to the IMU time axis. This application adopts a linear time model, as shown in equation (3): (3) In the formula, Representing the clock proportional compensation and fixed delay compensation of the sensor respectively, the solution is obtained by least squares estimation (see equation (4)): (4) In the formula, This indicates the IMU timestamp closest to the observation.

[0068] Step 3: Construction and distribution of synchronization measurement packets. After synchronization is complete, external observations can be assigned to the nearest IMU time according to the nearest neighbor principle (Equation (5)): (5) In the formula, This represents the synchronized timestamp of the j-th data item from the i-th type of sensor. This represents the m-th timestamp in the IMU stream.

[0069] And generate the corresponding synchronous measurement package, as shown in equation (6): (6) In the formula, The synchronous measurement packet represents the set of observations bound to the IMU at the k-th timestamp. These represent the sets of all GNSS and LiDAR observations associated with this IMU timestamp. In general, external observation timestamps are mapped to the IMU timeline using a synchronization model, and the corresponding IMU observation timestamps are obtained based on the nearest neighbor principle. External observation data is then bound to these timestamps to form a synchronization measurement packet.

[0070] The synchronization measurement package contains: current IMU measurements (for prediction), all time-aligned external observations (for updates), and relevant index information (for snapshot playback). The synchronization measurement package is then sent to subsequent filtering modules: FAST-ESKF (real-time prediction) and SLOW-ESKF (delayed updates and snapshot playback), providing a foundation for delay processing, conflict resolution, and computational decoupling in subsequent filtering.

[0071] Phase Two: FAST-ESKF High-Frequency Prediction Thread and Snapshot Update Mechanism; In this embodiment, a "snapshot" refers to a system state cache structure that is saved timestamp-based on the IMU's main timeline, used to support delayed observation playback and cross-thread state sharing. Each snapshot is stored at the IMU time. Defined as Equation (7): (7) In the formula, Represents the timestamp (current moment). Represents the nominal state (position, velocity, attitude, bias, etc.). Indicates the error status. This represents the error covariance matrix.

[0072] Step 1: Trigger snapshot recovery and state prediction based on IMU data.

[0073] The FAST-ESKF thread (real-time filtering thread) is awakened each time IMU data arrives. It searches the snapshot buffer for the latest available snapshot based on the current IMU timestamp, restores the historical state, and completes the prediction. At time... Take a snapshot, as in equation (8): (8) Then we can divide it into two cases: In the first case, if If the snapshot is outdated, the snapshot replay mechanism is executed, i.e., traversal... All synchronous measurement packets within the interval: Perform ESKF prediction on the IMU segment in each packet; Time state recursion to This forms the latest snapshot state, as shown in equation (9): (9) In the formula, This indicates the timestamp of the snapshot currently retrieved from the snapshot area (the moment of the first snapshot). This indicates the target time point to be restored to, i.e., the time of the "previous frame state," or the previous moment. This indicates that after the playback ends, at time [time] The predicted state is indicated by the superscript "-", which represents the "predicted value or prior value". Indicates at time The predicted covariance, This represents a snapshot playback - state backtracking operation, which is a process function that starts from a given initial state and replays the snapshot in chronological order. All the IMU data in this segment is used for ESKF prediction, and the state is ultimately propagated to time. , Indicates from time arrive All IMU measurement sequences between, that is, all synchronous measurement packets whose timestamps are located between the first snapshot time and the previous time, are found from the snapshot buffer.

[0074] In the second scenario, if If the snapshot is aligned with the previous frame, then IMU prediction is directly performed based on that snapshot, see equation (10): (10) In the formula, This represents the prediction function of ESKF. These represent the results after the current IMU prediction, at time [time value missing]. The obtained prior states and corresponding prior covariances.

[0075] Step 2: Snapshot Update and Output. Predicting the current time. Then, a new snapshot is generated, as shown in equation (11): (11) Simultaneously, the predicted state is used as the system output (as shown in equation (12)) for navigation or control: (12) In the formula, Indicates the system at the current moment Output attitude / state provided externally, superscript " "" indicates an estimated value.

[0076] The FAST-ESKF thread uses IMU observations as trigger signals to perform state prediction on the IMU timeline. It also uses a snapshot mechanism to restore historical states, perform incremental updates, and share states across threads, enabling high-frequency real-time output without waiting for external observations.

[0077] Phase 3: SLOW-ESKF delayed observation processing and snapshot backtracking update mechanism; Step 1: Delayed observation triggering and snapshot recovery.

[0078] When external observations (such as GNSS / LiDAR) arrive and their timestamps are available... Lagging behind the current moment At this time, the SLOW thread (delay filtering thread) is triggered. The system first retrieves a snapshot from the snapshot cache that matches... Recently but no later than A snapshot, as in equation (13): (13) Step 2: Backtracking update based on snapshot.

[0079] From snapshot time Initially, the SLOW thread replays all synchronization measurement packets in chronological order up to the current moment, representing the system state. Depending on whether the in-packet observation data includes external observations, the following steps are executed dynamically: if external observations (GNSS / LiDAR) exist, the ESKF update step is executed; if no observations exist, only the ESKF prediction step is executed.

[0080] The entire recursive process can be summarized as equation (14): (14) In the formula, These represent the delayed observation timestamp, the timestamp of the m-th synchronization measurement packet, and the current system time, respectively. These represent the filter's time... The predicted / intermediate state estimates and the corresponding error state covariance matrix, These are represented as the state estimate updated based on the synchronous measurement packet and the corresponding error state covariance matrix, respectively. These represent the IMU data sequences and external observations (GNSS / LiDAR) contained in the synchronous measurement package, respectively. If none of them exist, they are empty. This represents the ESKF prediction function, which performs state propagation based on IMU pre-integration. This indicates the ESKF update function, which performs measurement updates based on external observations.

[0081] The recursive process ultimately yields the updated estimate of the current time. This mechanism requires no modification to the state structure, no locking or waiting, allows for arbitrary delays in observation insertion, and has no impact on the real-time operation of the system.

[0082] Phase 4: Multi-threaded fusion output and robust consistency management; Step 1: Snapshot reference replacement achieves cross-thread state consistency maintenance. The FAST thread directly outputs the current snapshot state after each IMU prediction, while the SLOW thread, after completing the delayed observation update, uses the snapshot replay mechanism to backtrack the historical state to the observation time and re-estimate it, ultimately generating a new state snapshot. To ensure that the shared state between the two threads is always consistent, the SLOW thread does not directly replace the running state after completing the update, but updates the snapshot reference through atomic replacement, i.e., equation (15): (15) In the formula, This indicates the current output time of the FAST thread. These represent the state vector currently referenced and output by the FAST thread and its corresponding covariance matrix, respectively. These represent the latest state estimate and corresponding covariance matrix calculated by the SLOW thread based on delayed observations and replay. "Atomic replacement means updating the snapshot reference in a lock-free manner instead of modifying the memory content.

[0083] Furthermore, this asynchronous multi-threaded filtering localization method for vehicles can also perform observation quality assessment and dynamic selective fusion strategies. When the synchronous measurement package contains multiple observation sources (such as GNSS and LiDAR), the method dynamically determines whether to include the observation in the state update by evaluating the quality indicators of each observation source (such as signal strength SNR, geometric distribution DOP, point cloud density, residual size, etc.), and the formula can be Equation (16): (16) In the formula, This represents the overall quality index of the i-th observation source / observation quantity. This represents a quality evaluation function.

[0084] An observation is used for ESKF updates only if its quality meets an adaptive threshold; otherwise, it is temporarily discarded to avoid introducing significant biases into the system due to low-quality measurements. Furthermore, this mechanism does not interfere with snapshot playback and snapshot consistency mechanisms, and only plays a selective role in the processing of each synchronized measurement packet.

[0085] Furthermore, the asynchronous multi-threaded filtering localization method for vehicles may also include automatic degradation for observation failures and output trajectory smoothing guarantees.

[0086] To adapt to potential sensor failures or extreme latency in the navigation environment, this application introduces an adaptive degradation strategy: if the GNSS / LiDAR quality does not meet the threshold requirements for multiple consecutive frames, it automatically switches to a pure prediction mode that relies solely on the IMU; in pure prediction mode, once observations are restored, the SLOW thread automatically backtracks and updates the state without requiring a system restart. The system ensures trajectory continuity and prevents state transitions through snapshot playback and update mechanisms. The output state can be lightly smoothed to ensure that the system can still work stably and output highly reliable state results under conditions such as observation interruption, out-of-order, delay, and loss. It has high robustness and is suitable for embedded and real-time scenarios.

[0087] It should be noted that, for the sake of simplicity, the foregoing method embodiments are all described as a series of actions. However, those skilled in the art should understand that this application is not limited to the described order of actions, as some steps may be performed in other orders or simultaneously according to this application. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are preferred embodiments, and the actions and modules involved are not necessarily essential to this application.

[0088] In the above embodiments, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0089] Figure 2 This is a structural block diagram of an asynchronous multi-threaded filtering positioning device for a vehicle provided in one embodiment of this application, as shown below. Figure 2 As shown. The asynchronous multi-threaded filtering positioning device for vehicles in this embodiment may include an acquisition module 21, a synchronization module 22, a first prediction module 23, a second prediction module 24, and a correction module 25; The acquisition module 21 is used to acquire the main observation data of the main sensor mounted on the vehicle and the auxiliary observation data of at least one auxiliary sensor; Synchronization module 22 is used to synchronize all auxiliary observation data based on the time axis of the main sensor, construct a synchronization measurement package, and maintain a snapshot cache area arranged along the time axis; The first prediction module 23 is used to trigger a real-time filtering thread when it receives the current master observation data at the current moment, obtain the first snapshot with the latest timestamp before the current moment from the snapshot cache, determine the positioning status at the previous moment based on the first snapshot, and predict and output the positioning status at the current moment based on the positioning status at the previous moment and the current master observation data. The second prediction module 24 is used to, when receiving the auxiliary observation data, trigger a delay filtering thread, retrieve the second snapshot with the latest timestamp before the timestamp of the auxiliary observation data in the snapshot cache, and obtain target observation data from the synchronization measurement packet, and use the target observation data to predict the corrected positioning status at the current time; wherein, the target observation data is all synchronization measurement packets between the timestamp of the second snapshot and the current time; The correction module 25 is used to update the snapshot reference shared by the two filtering threads, so that the real-time filtering thread can obtain the corrected positioning state at the current moment obtained by the delayed filtering thread, in order to maintain the consistency of the states of the two filtering threads.

[0090] It should be noted that some or all of the asynchronous multi-threaded filtering and positioning device for vehicles in this embodiment can be an application located on a local terminal, or it can be a plugin or software development kit (SDK) or other functional unit set in an application located on a local terminal, or it can be a processing engine located on a network-side server, or it can be a distributed system located on the network side, such as a processing engine or distributed system in a network-side autonomous driving platform, etc. This embodiment does not impose any particular limitations on this.

[0091] It is understood that the application can be a native program installed on the local terminal, or it can be a web application of a browser on the local terminal. This embodiment does not limit this.

[0092] One embodiment of this application provides a computer-readable storage medium storing at least one instruction, which is loaded and executed by a processor to implement the asynchronous multi-threaded filtering positioning method for vehicles as described above.

[0093] One embodiment of this application provides an electronic device including a processor and a memory, wherein the memory stores at least one instruction, which is loaded and executed by the processor to implement the asynchronous multi-threaded filtering positioning method for vehicles as described above.

[0094] One embodiment of this application provides a vehicle including the electronic devices described above.

[0095] The collection, storage, use, processing, transmission, provision, and disclosure of user personal information involved in the technical solution of this application all comply with the provisions of relevant laws and regulations and do not violate public order and good morals.

[0096] Figure 3 This is a schematic block diagram of an example electronic device according to embodiments of this application. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device may also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the present application described and / or claimed herein.

[0097] like Figure 3 As shown, the electronic device 300 includes a computing unit 301, which can perform various appropriate actions and processes based on a computer program stored in a read-only memory (ROM) 302 or a computer program loaded from a storage unit 308 into a random access memory (RAM) 303. The RAM 303 may also store various programs and data required for the operation of the electronic device 300. The computing unit 301, ROM 302, and RAM 303 are interconnected via a bus 304. An input / output (I / O) interface 305 is also connected to the bus 304.

[0098] Multiple components in electronic device 300 are connected to I / O interface 305, including: input unit 306, such as keyboard, mouse, etc.; output unit 307, such as various types of displays, speakers, etc.; storage unit 308, such as disk, optical disk, etc.; and communication unit 309, such as network card, modem, wireless transceiver, etc. Communication unit 309 allows electronic device 300 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0099] The computing unit 301 can be various general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of the computing unit 301 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. The computing unit 301 performs the various methods and processes described above, such as an asynchronous multi-threaded filtering localization method for a vehicle. For example, in some embodiments, the asynchronous multi-threaded filtering localization method for a vehicle can be implemented as a computer software program tangibly contained in a machine-readable medium, such as storage unit 308. In some embodiments, part or all of the computer program can be loaded and / or installed on the electronic device 300 via ROM 302 and / or communication unit 309. When the computer program is loaded into RAM 303 and executed by the computing unit 301, one or more steps of the asynchronous multi-threaded filtering localization method for a vehicle described above can be performed. Alternatively, in other embodiments, the computing unit 301 may be configured by any other suitable means (e.g., by means of firmware) to perform an asynchronous multi-threaded filtering localization method for a vehicle.

[0100] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), complex programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations may include: implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transferring data and instructions to the storage system, at least one input device, and at least one output device.

[0101] The program code used to implement the methods of this application may be written in any combination of one or more programming languages. This program code may be provided to a processor or controller of a general-purpose computer, special-purpose computer, or other programmable data processing device, such that when executed by the processor or controller, the functions / operations specified in the flowcharts and / or block diagrams are implemented. The program code may be executed entirely on a machine, partially on a machine, as a standalone software package partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0102] In the context of this application, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. Machine-readable media can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0103] It should be understood that the various forms of processes shown above can be used to rearrange, add, or delete steps. For example, the steps described in this application can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this application can be achieved, and this is not limited herein.

[0104] The specific embodiments described above do not constitute a limitation on the scope of protection of this application. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. An asynchronous multi-threaded filtering localization method for vehicles, characterized in that, include: Acquire primary observation data from the main sensor mounted on the vehicle and auxiliary observation data from at least one auxiliary sensor; Based on the time axis of the main sensor, all auxiliary observation data are synchronized in time, a synchronization measurement package is constructed, and a snapshot cache area arranged along the time axis is maintained; When the current master observation data is received, the real-time filtering thread is triggered to obtain the first snapshot with the latest timestamp before the current moment from the snapshot buffer. Based on the first snapshot, the positioning status at the previous moment is determined, and based on the positioning status at the previous moment and the current master observation data, the positioning status at the current moment is predicted and output. When the auxiliary observation data is received, a delay filtering thread is triggered to retrieve the second snapshot with the latest timestamp before the timestamp of the auxiliary observation data in the snapshot buffer, and to obtain the target observation data from the synchronization measurement packet. Using the target observation data, the corrected positioning status at the current moment is predicted; wherein, the target observation data is all synchronization measurement packets between the timestamp of the second snapshot and the current moment. By updating the snapshot reference shared by the two filtering threads, the real-time filtering thread can obtain the corrected positioning state at the current moment obtained by the delayed filtering thread, thereby maintaining the consistency of the states of the two filtering threads.

2. The asynchronous multi-threaded filtering positioning method for vehicles according to claim 1, characterized in that, Based on the time axis of the main sensor, all auxiliary observation data are synchronized in time to construct a synchronization measurement package, including: The main observation data and each auxiliary observation data are respectively converted into their respective standardized data structures; Based on the time axis of the main sensor, a preset linear time model and least squares estimation are used to perform clock ratio compensation and fixed delay compensation on the original timestamp of each auxiliary observation data to obtain the synchronization timestamp of each auxiliary observation data. Based on the nearest neighbor principle, each synchronization timestamp is assigned to the observation time of the nearest master sensor; All sub-auxiliary observation data allocated to the observation time of the main sensor are bound to the sub-main observation data of the observation time of the main sensor to form the synchronous measurement package; Maintaining snapshot caches arranged along the time axis includes: At each observation time of the main sensor, a snapshot is generated, including the timestamp, nominal state, error state, and error covariance matrix of each observation time; Snapshots of each observation moment are stored in the snapshot cache area in order of timestamp.

3. The asynchronous multi-threaded filtering positioning method for vehicles according to claim 1, characterized in that, The positioning status at the previous moment is determined based on the first snapshot, and the positioning status at the current moment is predicted and output based on the positioning status at the previous moment and the current master observation data, including: When the timestamp of the first snapshot is equal to the timestamp of the previous moment, the positioning status in the first snapshot is used as the positioning status of the previous moment. When the timestamp of the first snapshot is earlier than the timestamp of the previous moment, the snapshot playback mechanism is triggered: all synchronous measurement packets in the snapshot buffer from the timestamp of the first snapshot to the timestamp of the previous moment are traversed, and error state Kalman filter prediction is performed on the main sensor observation data in each synchronous measurement packet in the order of timestamps. The state in the first snapshot is then pushed back to the previous moment to obtain the positioning state of the previous moment. Based on the positioning state of the previous moment, and combined with the current master observation data, an error state Kalman filter prediction is performed to obtain the prior positioning state of the current moment, which is then output as the current positioning state.

4. The asynchronous multi-threaded filtering positioning method for vehicles according to claim 1, characterized in that, Predicting the corrected positioning state at the current moment using the target observation data includes: Each synchronous measurement packet in the target observation data is processed sequentially according to the timestamp order, and the estimated state at the current time obtained after recursive processing of all synchronous measurement packets is used as the corrected positioning state at the current time. In the process of processing each synchronous measurement package in the target observation data, if the current synchronous measurement package contains auxiliary observation data, then an error state Kalman filter update is performed based on the auxiliary observation data; if the current synchronous measurement package does not contain auxiliary observation data, then an error state Kalman filter prediction is performed based on the master observation data in the current synchronous measurement package.

5. The asynchronous multi-threaded filtering positioning method for vehicles according to claim 4, characterized in that, Also includes: When the delay filtering thread processes the synchronous measurement package, it calculates a comprehensive quality index for each auxiliary observation data in the synchronous measurement package; wherein, the comprehensive quality index is calculated by a preset quality evaluation function based on at least one parameter among the signal strength, geometric distribution accuracy factor, point cloud density, and residual size of the auxiliary observation data; Determine whether the comprehensive quality index meets the preset adaptive threshold; If the conditions are met, the auxiliary observation data is retained for use in the error state Kalman filter update. If the conditions are not met, the auxiliary observation data shall be discarded.

6. The asynchronous multi-threaded filtering positioning method for vehicles according to claim 5, characterized in that, Also includes: When the comprehensive quality index of a first preset number of consecutive auxiliary observation data does not meet the adaptive threshold, the process of the delayed filtering thread predicting the corrected positioning status at the current moment is paused, and the positioning status at the current moment is continued to be predicted and output only by the real-time filtering thread. When the comprehensive quality index of a second consecutive preset number of auxiliary observation data is detected to meet the adaptive threshold, the delay filtering thread is triggered again to perform the process of predicting the corrected positioning state at the current moment.

7. The asynchronous multi-threaded filtering and positioning method for vehicles according to claim 1, characterized in that, By updating the snapshot reference shared by the two filtering threads, the real-time filtering thread can obtain the corrected positioning status at the current moment obtained by the delayed filtering thread, including: An atomic replacement method is used to update the snapshot reference output by the real-time filtering thread to point to the snapshot output by the delayed filtering thread, so that subsequent prediction operations of the real-time filtering thread are executed based on the corrected positioning state in the snapshot output by the delayed filtering thread.

8. An asynchronous multi-threaded filtering and positioning device for vehicles, characterized in that, include: The acquisition module is used to acquire the main observation data from the main sensor mounted on the vehicle and the auxiliary observation data from at least one auxiliary sensor. The synchronization module is used to synchronize all auxiliary observation data in time based on the time axis of the main sensor, construct a synchronization measurement package, and maintain a snapshot cache area arranged along the time axis; The first prediction module is used to trigger a real-time filtering thread when it receives the current master observation data at the current moment, obtain the first snapshot with the latest timestamp before the current moment from the snapshot cache, determine the positioning status at the previous moment based on the first snapshot, and predict and output the positioning status at the current moment based on the positioning status at the previous moment and the current master observation data. The second prediction module is used to trigger a delay filtering thread when the auxiliary observation data is received, retrieve the second snapshot with the latest timestamp before the timestamp of the auxiliary observation data in the snapshot cache, and obtain target observation data from the synchronization measurement packet, and use the target observation data to predict the corrected positioning status at the current time; wherein, the target observation data is all synchronization measurement packets between the timestamp of the second snapshot and the current time; The correction module is used to update the snapshot reference shared by the two filtering threads, so that the real-time filtering thread can obtain the corrected positioning status at the current moment obtained by the delayed filtering thread, thereby maintaining the consistency of the states of the two filtering threads.

9. An electronic device, characterized in that, include: At least one processor; as well as A memory communicatively connected to the at least one processor; wherein, The memory stores instructions executable by the at least one processor, which, when executed by the at least one processor, enables the at least one processor to perform the asynchronous multi-threaded filtering localization method for a vehicle according to any one of claims 1-8.

10. A vehicle, characterized in that, Including the electronic device as described in claim 9.

Images