Time synchronization method, device and equipment for multi-sensor fusion positioning system

Abstract

The present application provides a time synchronization method, device and equipment for a multi-sensor fusion positioning system, the method comprising: for each type of sensor data, storing a certain number of the latest received sensor data in a circular queue; calculating the synchronization time difference according to the time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue; calling each data processing module of the multi-sensor fusion positioning system at a certain frequency, calculating the synchronization timestamp according to the calling frequency, calling timestamp and synchronization time difference of the data processing module, and passing the synchronization timestamp into the data processing module. Through this application, the fusion positioning result output by the multi-sensor fusion positioning system is guaranteed to be stable in period, and during real-time and offline re-injection operation, the timestamp in the fusion positioning result and the standard timestamp of the sensor data are kept at the same time reference.

Description

Technical Field

[0001] The present application relates to the technical field of multi-sensor fusion positioning technology, and in particular to a time synchronization method, device and equipment for a multi-sensor fusion positioning system. Background Art

[0002] The multi-sensor fusion positioning system needs to ensure that the output fusion positioning result cycle is stable, but the input frequency of sensor data is unstable, such as delay, frame loss, etc. In severe cases, there may be no sensor data input for a long time. In addition, based on the sensor and fusion positioning result data packets collected by the real vehicle, when the fusion positioning system is run offline, it is necessary to ensure that when the same sensor data is input, the fusion positioning result output by the fusion positioning system is consistent with the real vehicle (data frame number, timestamp, location and other attributes). Therefore, how to effectively and stably output the fusion positioning result on the same time basis as the sensor data when running the fusion positioning system in real time and offline re-injection is a technical problem that needs to be solved urgently. Summary of the invention

[0003] The present application provides a time synchronization method, device and equipment for a multi-sensor fusion positioning system, which can solve the technical problem that when the fusion positioning system is operated in real time and offline re-injection, the fusion positioning result under the same time reference as the sensor data cannot be effectively and stably output.

[0004] In a first aspect, an embodiment of the present application provides a time synchronization method for a multi-sensor fusion positioning system, and the time synchronization method for a multi-sensor fusion positioning system includes:

[0005] For each type of sensor data, a certain number of the most recently received sensor data are stored in a circular queue, wherein the sensor data all have a standard timestamp, and the IMU data also has a receiving timestamp added when it is received;

[0006] The synchronization time difference is calculated based on the time difference between the standard timestamp and the receiving timestamp of all IMU data in the circular queue;

[0007] Each data processing module of the multi-sensor fusion positioning system is called at a certain frequency, and the synchronization timestamp is calculated according to the calling frequency, calling timestamp and synchronization time difference of the data processing module. The synchronization timestamp is passed into the data processing module, so that the data processing module obtains the sensor data corresponding to the standard timestamp and the synchronization timestamp from the circular queue for data processing, and uses the synchronization timestamp as the timestamp of the output result.

[0008] Further, in one embodiment, the step of calculating the synchronization time difference according to the time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue comprises:

[0009] If there is a synchronization time difference, the difference between the synchronization time difference and the time difference between the standard timestamp and the receiving timestamp of the IMU data in the circular queue is calculated in sequence;

[0010] If the absolute value of the difference is greater than the deviation threshold, the corresponding IMU data is taken as a mutation frame, otherwise the corresponding IMU data is taken as a valid frame;

[0011] If the maximum number of consecutive frames of mutation frames in the circular queue is less than N, and the interval time from the last calculation of the synchronization time difference reaches the update period, then the average time difference between the standard timestamp and the received timestamp of all valid frames in the circular queue is calculated to obtain a new synchronization time difference, where N is a positive integer greater than 1.

[0012] Further, in one embodiment, after the step of taking the corresponding IMU data as a mutation frame if the absolute value of the difference is greater than the deviation threshold, and taking the corresponding IMU data as a valid frame otherwise, the step further includes:

[0013] If N consecutive mutation frames are detected, all IMU data before these N mutation frames are removed from the circular queue, and the average time difference between the standard timestamp and the received timestamp of all remaining IMU data is calculated to obtain a new synchronization time difference.

[0014] Further, in one embodiment, the step of calculating the synchronization time difference according to the time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue comprises:

[0015] If there is no synchronization time difference, the average time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue is calculated to obtain the synchronization time difference.

[0016] Furthermore, in one embodiment, the formula for calculating the synchronization timestamp is:

[0017] timestamp=(uint64_t((time+1e-6)*frequency))*(1.0 / frequency)

[0018] Among them, timestamp represents the synchronization timestamp, time represents the sum of the call timestamp and the synchronization time difference, the type is double-precision floating point type, the unit is seconds, frequency represents the preset frequency, the type is integer type, the unit is Hz, the value of 1e-6 is 0.000001, and uint64_t represents the forced conversion of double-precision floating point type to integer type.

[0019] Furthermore, in one embodiment, the time synchronization method of the multi-sensor fusion positioning system further includes:

[0020] When the interval time from the last IMU data reception reaches the delay threshold, the data processing module is suspended, all sensor data caches are cleared, and the synchronization time difference is deleted;

[0021] When the IMU data is received again, the synchronization time difference is recalculated, the data processing module is deinitialized, and the data processing module is resumed.

[0022] In a second aspect, an embodiment of the present application further provides a time synchronization device for a multi-sensor fusion positioning system, the time synchronization device for the multi-sensor fusion positioning system comprising:

[0023] A cache module is used to store a certain number of sensor data received recently into a circular queue for each type of sensor data, wherein the sensor data all have a standard timestamp, and the IMU data also has a receiving timestamp added when receiving;

[0024] The time difference calculation module is used to calculate the synchronization time difference according to the time difference between the standard timestamp and the receiving timestamp of all IMU data in the circular queue;

[0025] The calling module is used to call each data processing module of the multi-sensor fusion positioning system at a certain frequency, calculate the synchronization timestamp according to the calling frequency, calling timestamp and synchronization time difference of the data processing module, and pass the synchronization timestamp into the data processing module, so that the data processing module obtains the sensor data corresponding to the standard timestamp and the synchronization timestamp from the circular queue for data processing, and uses the synchronization timestamp as the timestamp of the output result.

[0026] Furthermore, in one embodiment, the time difference calculation module is used to:

[0027] If there is a synchronization time difference, the difference between the synchronization time difference and the time difference between the standard timestamp and the receiving timestamp of the IMU data in the circular queue is calculated in sequence;

[0028] If the absolute value of the difference is greater than the deviation threshold, the corresponding IMU data is taken as a mutation frame, otherwise the corresponding IMU data is taken as a valid frame;

[0029] If the maximum number of consecutive frames of mutation frames in the circular queue is less than N, and the interval time from the last calculation of the synchronization time difference reaches the update period, then the average time difference between the standard timestamp and the received timestamp of all valid frames in the circular queue is calculated to obtain a new synchronization time difference, where N is a positive integer greater than 1.

[0030] Furthermore, in one embodiment, the time difference calculation module is also used for:

[0031] If N consecutive mutation frames are detected, all IMU data before these N mutation frames are removed from the circular queue, and the average time difference between the standard timestamp and the received timestamp of all remaining IMU data is calculated to obtain a new synchronization time difference.

[0032] In a third aspect, an embodiment of the present application also provides a time synchronization device for a multi-sensor fusion positioning system, wherein the time synchronization device for the multi-sensor fusion positioning system comprises a processor, a memory, and a time synchronization program stored in the memory and executable by the processor, wherein when the time synchronization program is executed by the processor, the steps of the time synchronization method for the multi-sensor fusion positioning system are implemented.

[0033] Through this application, the synchronization time difference is calculated quickly and accurately by utilizing the relatively high frequency and stability of IMU data among all sensor data. When calling the data processing module, the synchronization timestamp calculated based on the calling frequency, calling timestamp and synchronization time difference is passed in. The data processing module selects sensor data and assigns timestamps to output results based on the synchronization timestamp, thereby ensuring that the fusion positioning result output by the multi-sensor fusion positioning system has a stable period, and during real-time and offline re-injection operation, the timestamp in the fusion positioning result and the standard timestamp of the sensor data are kept on the same time base. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 This is a flow chart of a time synchronization method for a multi-sensor fusion positioning system in one embodiment of the present application;

[0035] Figure 2 for Figure 1 Detailed flow chart of step S12;

[0036] Figure 3 This is a schematic diagram of the functional modules of a time synchronization device for a multi-sensor fusion positioning system in one embodiment of the present application;

[0037] Figure 4 This is a schematic diagram of the hardware structure of the time synchronization device of the multi-sensor fusion positioning system involved in the embodiment of the present application. DETAILED DESCRIPTION

[0038] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0039] The terms "including" and "having" and any variations thereof in the specification and claims of this application and the above-mentioned drawings are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but optionally includes steps or units that are not listed, or optionally includes other steps or units inherent to these processes, methods, products or devices. The terms "first", "second" and "third" are used to distinguish different objects, etc., and do not represent a sequence, nor do they limit "first", "second" and "third" to different types.

[0040] In the description of the embodiments of the present application, "exemplary", "for example" or "for example" are used to indicate examples, illustrations or descriptions. Any embodiment or design described as "exemplary", "for example" or "for example" in the embodiments of the present application should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of words such as "exemplary", "for example" or "for example" is intended to present related concepts in a specific way.

[0041] In the description of the embodiments of the present application, unless otherwise specified, “ / ” means or, for example, A / B can mean A or B; the “and / or” in the text is merely a description of the association relationship of associated objects, indicating that three relationships may exist, for example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. In addition, in the description of the embodiments of the present application, “multiple” refers to two or more than two.

[0042] In some processes described in the embodiments of the present application, multiple operations or steps that appear in a specific order are included, but it should be understood that these operations or steps may not be executed in the order in which they appear in the embodiments of the present application or in parallel, and the sequence number of the operation is only used to distinguish the different operations, and the sequence number itself does not represent any execution order. In addition, these processes may include more or fewer operations, and these operations or steps may be executed in sequence or in parallel, and these operations or steps may be combined.

[0043] In order to make the objectives, technical solutions and advantages of the present application clearer, the implementation methods of the present application will be further described in detail below with reference to the accompanying drawings.

[0044] In a first aspect, an embodiment of the present application provides a time synchronization method for a multi-sensor fusion positioning system.

[0045] Figure 1 A schematic flow chart of a time synchronization method for a multi-sensor fusion positioning system in an embodiment of the present application is shown.

[0046] Reference Figure 1 In one embodiment, a time synchronization method for a multi-sensor fusion positioning system includes the following steps:

[0047] S11. For each type of sensor data, a certain number of sensor data that are recently received are stored in a circular queue, wherein the sensor data all have a standard timestamp, and the IMU (Inertial measurement unit) data also has a receiving timestamp added when it is received.

[0048] In this embodiment, a certain amount of buffering is performed on all sensor data that need to be input into the multi-sensor fusion positioning system. When the input frequency of each sensor data is unstable, the data processing module of the multi-sensor fusion positioning system can select sensor data with time matching. A circular queue is used for data caching, and the vector space is designed as a circular ring connected end to end. The queue size is fixed, which can effectively prevent the "false overflow" phenomenon. Based on the circular queue, the external interfaces that need to be encapsulated include storing data, obtaining the latest frame or multiple frames of data, and obtaining all data after the specified system timestamp.

[0049] For example, the sensor data includes IMU data, vehicle body data, GNSS (Global Navigation Satellite System) / RTK (Real-time kinematic) data, etc., and the data processing module includes an integrated navigation module, etc. All sensor data received in this step have their own standard timestamps, such as UTC (Coordinated Universal Time) timestamps.

[0050] In particular, when receiving IMU data, a receiving timestamp is also added to it. The receiving timestamp is determined based on the clock of the device running this method, and there is usually a time difference with the standard timestamp. For example, when running offline, the sensor data may be one month old, while the device clock is the time when the test was performed. When running in real time, time differences may also occur due to insufficient accuracy of the device clock.

[0051] It should be noted that the number of caches for each sensor data can be set as needed. Since the IMU data will be used to calculate the synchronization time difference in subsequent steps, the number of caches for IMU data will be greater than that for other sensor data. Optionally, the number of caches can be determined based on the average input frequency of the sensor data. For example, if the average input frequency of IMU data is 100HZ, and the number of caches is set to 1000, about 10 seconds of IMU data can be stored. While occupying an appropriate amount of memory, the synchronization time difference can be calculated more accurately and quickly. For other sensor data, a cache is opened to store 1 to 2 seconds of corresponding data.

[0052] S12. Calculate the synchronization time difference according to the time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue.

[0053] In this embodiment, the relatively high frequency and stability of IMU data among all sensor data are utilized to quickly and accurately calculate the synchronization time difference. The synchronization time difference is used to characterize the time deviation between the device clock and the standard time. In subsequent steps, it is used to eliminate the impact of the time deviation, so that the timestamp in the fused positioning result can be kept on the same time basis as the standard timestamp of the sensor data.

[0054] S13. Call each data processing module of the multi-sensor fusion positioning system at a certain frequency, calculate the synchronization timestamp according to the calling frequency of the data processing module, the calling timestamp and the synchronization time difference, and pass the synchronization timestamp into the data processing module, so that the data processing module obtains the sensor data corresponding to the standard timestamp and the synchronization timestamp from the circular queue for data processing, and uses the synchronization timestamp as the timestamp of the output result.

[0055] In this embodiment, the call timestamp is determined based on the clock of the device running this method. The output frequency of the data processing module is ensured to be stable by calling the data processing module at a certain frequency. The synchronization timestamp calculated according to the calling frequency, calling timestamp and synchronization time difference of the data processing module is kept at the same time reference as the standard timestamp of the sensor data, so that the data processing module can accurately select the sensor data with time matching from the sensor data cached within a period of time, and assign the timestamp to the output result.

[0056] Therefore, through this embodiment, the relatively high frequency and stability of IMU data among all sensor data are utilized to quickly and accurately calculate the synchronization time difference. When calling the data processing module, the synchronization timestamp calculated according to the calling frequency, calling timestamp and synchronization time difference is passed in. The data processing module selects sensor data and assigns timestamps to output results according to the synchronization timestamp, thereby ensuring that the fusion positioning result output by the multi-sensor fusion positioning system is stable in periodicity, and during real-time and offline re-injection operation, the timestamp in the fusion positioning result and the standard timestamp of the sensor data are kept at the same time reference.

[0057] Figure 2 Shows Figure 1 Detailed flowchart of step S12 in FIG.

[0058] Reference Figure 2 In one embodiment, step S12 specifically includes:

[0059] S121. If there is a synchronization time difference, the difference between the synchronization time difference and the time difference between the standard timestamp of the IMU data in the circular queue and the receiving timestamp is calculated in sequence;

[0060] S122, if the absolute value of the difference is greater than the deviation threshold, the corresponding IMU data is used as a mutation frame, otherwise the corresponding IMU data is used as a valid frame;

[0061] S123a, if the maximum number of consecutive frames of the mutation frame in the circular queue is less than N, and the interval time from the last calculation of the synchronization time difference reaches the update period, then the average value of the time difference between the standard timestamp and the receiving timestamp of all valid frames in the circular queue is calculated to obtain a new synchronization time difference, where N is a positive integer greater than 1;

[0062] S123b. If N consecutive mutation frames are detected, all IMU data before the N mutation frames are removed from the circular queue, and the average time difference between the standard timestamp and the receiving timestamp of all remaining IMU data is calculated to obtain a new synchronization time difference.

[0063] In this embodiment, step S121 and step S122 are steps required to determine whether it is necessary to recalculate and update the synchronization time difference after the synchronization time difference has been calculated. Since there is a calculated synchronization time difference, it can be determined whether the IMU data has a sudden change according to the absolute value of the difference calculated in step S121, that is, it does not conform to the time deviation represented by the current synchronization time difference. The deviation threshold can be determined according to the normal transmission delay of the IMU data. For example, under normal circumstances, the transmission delay of the IMU data should be within 100 milliseconds, and the deviation threshold is set to 100 milliseconds.

[0064] After detecting all IMU data in the circular queue, no mutation is found in N consecutive frames or more of IMU data, indicating that the mutation of the IMU data is an occasional mutation, and the overall trend of the IMU data has not changed. Step S123a is executed according to the timing update strategy, and the synchronization time difference is updated when the update cycle is reached. When calculating the average value, only valid frames are selected to improve accuracy. For example, N can be set to 5.

[0065] If the update cycle is too short, the synchronization timestamp calculated when calling the data processing module after the update may be repeated or regressed compared with the synchronization timestamp calculated before the update. If the update cycle is too long, the synchronization timestamps before and after the update may jump greatly, and the output frequency of the data processing module may be unstable. After testing, the effect is better when the update cycle is set to ten minutes.

[0066] In the process of detecting each IMU data in the circular queue in turn, N consecutive mutation frames are detected, indicating that the overall trend of the IMU data has changed. According to the instant update strategy, step S123b is executed to eliminate all IMU data before the overall trend changes, that is, all IMU data before these N mutation frames, and calculate the average time difference between the standard timestamp and the receiving timestamp of all remaining IMU data to obtain the synchronization time difference after the overall trend changes. For example, there are 1000 frames of IMU data in the circular queue, and 5 consecutive mutation frames are detected starting from the 500th frame, then the average time difference of the time difference between the 500th frame and the 1000th frame of IMU data is used as the new synchronization time difference.

[0067] Furthermore, in one embodiment, step S12 specifically includes:

[0068] If there is no synchronization time difference, the average time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue is calculated to obtain the synchronization time difference.

[0069] In this embodiment, since there is no calculated synchronization time difference, it is impossible to determine whether the IMU data is a mutation frame, and the average value of the time difference of all IMU data is used as the synchronization time difference.

[0070] Furthermore, in one embodiment, the formula for calculating the synchronization timestamp is:

[0071] timestamp=(uint64_t((time+1e-6)*frequency))*(1.0 / frequency)

[0072] Among them, timestamp represents the synchronization timestamp, time represents the sum of the call timestamp and the synchronization time difference, the type is double-precision floating point type, the unit is seconds, frequency represents the preset frequency, the type is integer type, the unit is Hz, the value of 1e-6 is 0.000001, and uint64_t represents the forced conversion of double-precision floating point type to integer type.

[0073] In this embodiment, when calling each data processing module thread, the timestamp corresponding to the sum of the calling timestamp and the synchronization time difference is often not stable. The reasons may be thread execution timeout, errors in the calculation of the remaining sleep time of the thread, changes in the soft synchronization time deviation, and thread scheduling blockage. In order to facilitate the data processing modules to publish, subscribe and verify data based on the synchronization timestamp, the sum of the calling timestamp and the synchronization time difference needs to be specially processed according to the calling frequency. After processing according to this method, the obtained synchronization timestamp is an integer multiple of the module output cycle.

[0074] Furthermore, in one embodiment, the time synchronization method of the multi-sensor fusion positioning system further includes:

[0075] When the interval time from the last IMU data reception reaches the delay threshold, the data processing module is suspended, all sensor data caches are cleared, and the synchronization time difference is deleted;

[0076] When the IMU data is received again, the synchronization time difference is recalculated, the data processing module is deinitialized, and the data processing module is resumed.

[0077] In this embodiment, when the IMU data reception interval is too long, it indicates that there is a problem with the sensor data input. If the new data is used on the original basis, it may cause a large deviation in data processing. Through the above steps, it is ensured that the multi-sensor fusion positioning system outputs a stable and effective fusion positioning result. For example, the delay threshold is set to 300 milliseconds.

[0078] In a second aspect, an embodiment of the present application also provides a time synchronization device for a multi-sensor fusion positioning system.

[0079] Figure 3 A schematic diagram of functional modules of a time synchronization device for a multi-sensor fusion positioning system in an embodiment of the present application is shown.

[0080] Reference Figure 3 In one embodiment, the time synchronization device of the multi-sensor fusion positioning system includes:

[0081] A cache module 10 is used to store a certain number of sensor data received recently into a circular queue for each type of sensor data, wherein the sensor data all have a standard timestamp, and the IMU data also has a receiving timestamp added when receiving;

[0082] The time difference calculation module 20 is used to calculate the synchronization time difference according to the time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue;

[0083] The calling module 30 is used to call each data processing module of the multi-sensor fusion positioning system at a certain frequency, calculate the synchronization timestamp according to the calling frequency of the data processing module, the calling timestamp and the synchronization time difference, and pass the synchronization timestamp to the data processing module, so that the data processing module obtains the sensor data corresponding to the standard timestamp and the synchronization timestamp from the circular queue for data processing, and uses the synchronization timestamp as the timestamp of the output result.

[0084] Furthermore, in one embodiment, the time difference calculation module 20 is used to:

[0085] If there is a synchronization time difference, the difference between the synchronization time difference and the time difference between the standard timestamp and the receiving timestamp of the IMU data in the circular queue is calculated in sequence;

[0086] If the absolute value of the difference is greater than the deviation threshold, the corresponding IMU data is taken as a mutation frame, otherwise the corresponding IMU data is taken as a valid frame;

[0087] If the maximum number of consecutive frames of mutation frames in the circular queue is less than N, and the interval time from the last calculation of the synchronization time difference reaches the update period, then the average time difference between the standard timestamp and the received timestamp of all valid frames in the circular queue is calculated to obtain a new synchronization time difference, where N is a positive integer greater than 1.

[0088] Furthermore, in one embodiment, the time difference calculation module 20 is also used for:

[0089] If N consecutive mutation frames are detected, all IMU data before these N mutation frames are removed from the circular queue, and the average time difference between the standard timestamp and the received timestamp of all remaining IMU data is calculated to obtain a new synchronization time difference.

[0090] Furthermore, in one embodiment, the time difference calculation module 20 is used to:

[0091] If there is no synchronization time difference, the average time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue is calculated to obtain the synchronization time difference.

[0092] Further, in one embodiment, the formula for calculating the synchronization timestamp in the calling module 30 is:

[0093] timestamp=(uint64_t((time+1e-6)*frequency))*(1.0 / frequency)

[0094] Among them, timestamp represents the synchronization timestamp, time represents the sum of the call timestamp and the synchronization time difference, the type is double-precision floating point type, the unit is seconds, frequency represents the preset frequency, the type is integer type, the unit is Hz, the value of 1e-6 is 0.000001, and uint64_t represents the forced conversion of double-precision floating point type to integer type.

[0095] Furthermore, in one embodiment, the time synchronization device of the multi-sensor fusion positioning system further includes a reset module, which is used to:

[0096] When the interval time from the last IMU data reception reaches the delay threshold, the data processing module is suspended, all sensor data caches are cleared, and the synchronization time difference is deleted;

[0097] When the IMU data is received again, the synchronization time difference is recalculated, the data processing module is deinitialized, and the data processing module is resumed.

[0098] Among them, the functional implementation of each module in the time synchronization device of the above-mentioned multi-sensor fusion positioning system corresponds to the various steps in the embodiment of the time synchronization method of the above-mentioned multi-sensor fusion positioning system, and its functions and implementation processes are no longer repeated here one by one.

[0099] In a third aspect, an embodiment of the present application provides a time synchronization device for a multi-sensor fusion positioning system. The time synchronization device for the multi-sensor fusion positioning system may be a personal computer (PC), a laptop computer, a server, or other device with data processing capabilities.

[0100] Figure 4 A schematic diagram of the hardware structure of a time synchronization device of a multi-sensor fusion positioning system involved in an embodiment of the present application is shown.

[0101] Reference Figure 4 In an embodiment of the present application, a time synchronization device of a multi-sensor fusion positioning system may include a processor, a memory, a communication interface, and a communication bus.

[0102] The communication bus may be of any type and is used to interconnect the processor, the memory, and the communication interface.

[0103] The communication interface includes input / output (I / O) interface, physical interface and logical interface, etc., which are used to realize the interconnection of devices inside the time synchronization device of the multi-sensor fusion positioning system, and the interface used to realize the interconnection between the time synchronization device of the multi-sensor fusion positioning system and other devices (such as other computing devices or user devices). The physical interface can be an Ethernet interface, a fiber optic interface, an ATM interface, etc.; the user device can be a display, a keyboard, etc.

[0104] The memory can be various types of storage media, such as random access memory (RAM), read-only memory (ROM), non-volatile RAM (NVRAM), flash memory, optical storage, hard disk, programmable ROM (PROM), erasable PROM (EPROM), electrically erasable PROM (EEPROM), etc.

[0105] The processor may be a general-purpose processor, and the general-purpose processor may call the time synchronization program stored in the memory and execute the time synchronization method of the multi-sensor fusion positioning system provided in the embodiment of the present application. For example, the general-purpose processor may be a central processing unit (CPU). The method executed when the time synchronization program is called may refer to the various embodiments of the time synchronization method of the multi-sensor fusion positioning system of the present application, which will not be repeated here.

[0106] Those skilled in the art will understand that Figure 4 The hardware structure shown in the figure does not constitute a limitation on the present application, and may include more or less components than shown in the figure, or combine certain components, or arrange the components differently.

[0107] It should be noted that the serial numbers of the above-mentioned embodiments of the present application are only for description and do not represent the advantages or disadvantages of the embodiments.

[0108] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present application is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, disk, CD) as described above, and includes a number of instructions for a terminal device to execute the methods described in each embodiment of the present application.

[0109] The above are only preferred embodiments of the present application, and are not intended to limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A time synchronization method for a multi-sensor fusion positioning system, characterized in that: The time synchronization method of the multi-sensor fusion positioning system includes: For each type of sensor data, a certain number of the most recently received sensor data are stored in a circular queue, wherein the sensor data all have a standard timestamp, and the IMU data also has a receiving timestamp added when receiving, and the receiving timestamp is determined based on the clock of the device running this method; The synchronization time difference is calculated based on the time difference between the standard timestamp and the receiving timestamp of all IMU data in the circular queue; The data processing modules of the multi-sensor fusion positioning system are called at a certain frequency, and the synchronization timestamp is calculated according to the calling frequency, calling timestamp and synchronization time difference of the data processing module. The synchronization timestamp is passed to the data processing module, so that the data processing module obtains the sensor data corresponding to the standard timestamp and the synchronization timestamp from the circular queue for data processing, and uses the synchronization timestamp as the timestamp of the output result, wherein the calling timestamp is determined based on the device clock running this method, and the formula for calculating the synchronization timestamp is: timestamp=(uint64_t((time+1e-6)*frequency))*(1.0 / frequency) Among them, timestamp represents the synchronization timestamp, time represents the sum of the call timestamp and the synchronization time difference, the type is double-precision floating point type, the unit is seconds, frequency represents the preset frequency, the type is integer type, the unit is Hz, the value of 1e-6 is 0.000001, and uint64_t represents the forced conversion of double-precision floating point type to integer type.

2. The time synchronization method of the multi-sensor fusion positioning system according to claim 1, characterized in that: The step of calculating the synchronization time difference according to the time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue comprises: If there is a synchronization time difference, the difference between the synchronization time difference and the time difference between the standard timestamp and the receiving timestamp of the IMU data in the circular queue is calculated in sequence; If the absolute value of the difference is greater than the deviation threshold, the corresponding IMU data is taken as a mutation frame, otherwise the corresponding IMU data is taken as a valid frame; If the maximum number of consecutive frames of mutation frames in the circular queue is less than N, and the interval time from the last calculation of the synchronization time difference reaches the update period, then the average time difference between the standard timestamp and the received timestamp of all valid frames in the circular queue is calculated to obtain a new synchronization time difference, where N is a positive integer greater than 1.

3. The time synchronization method of the multi-sensor fusion positioning system according to claim 2, characterized in that: After the step of taking the corresponding IMU data as a mutation frame if the absolute value of the difference is greater than the deviation threshold, and taking the corresponding IMU data as a valid frame otherwise, the method further includes: If N consecutive mutation frames are detected, all IMU data before these N mutation frames are removed from the circular queue, and the average time difference between the standard timestamp and the received timestamp of all remaining IMU data is calculated to obtain a new synchronization time difference.

4. The time synchronization method of the multi-sensor fusion positioning system according to claim 1, characterized in that: The step of calculating the synchronization time difference according to the time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue comprises: If there is no synchronization time difference, the average time difference between the standard timestamp and the received timestamp of all IMU data in the circular queue is calculated to obtain the synchronization time difference.

5. The time synchronization method of the multi-sensor fusion positioning system according to claim 1, characterized in that: The time synchronization method of the multi-sensor fusion positioning system also includes: When the interval time from the last IMU data reception reaches the delay threshold, the data processing module is suspended, all sensor data caches are cleared, and the synchronization time difference is deleted; When the IMU data is received again, the synchronization time difference is recalculated, the data processing module is deinitialized, and the data processing module is resumed.

6. A time synchronization device for a multi-sensor fusion positioning system, characterized in that: The time synchronization device of the multi-sensor fusion positioning system comprises: A cache module is used to store a certain number of sensor data received recently in a circular queue for each type of sensor data, wherein the sensor data all have a standard timestamp, and the IMU data also has a receiving timestamp added when receiving, and the receiving timestamp is determined based on the clock of the device running the method; The time difference calculation module is used to calculate the synchronization time difference according to the time difference between the standard timestamp and the receiving timestamp of all IMU data in the circular queue; The calling module is used to call each data processing module of the multi-sensor fusion positioning system at a certain frequency, calculate the synchronization timestamp according to the calling frequency, calling timestamp and synchronization time difference of the data processing module, and pass the synchronization timestamp into the data processing module, so that the data processing module obtains the sensor data corresponding to the standard timestamp and the synchronization timestamp from the circular queue for data processing, and uses the synchronization timestamp as the timestamp of the output result, wherein the calling timestamp is determined based on the device clock running this method, and the formula for calculating the synchronization timestamp is: timestamp=(uint64_t((time+1e-6)*frequency))*(1.0 / frequency) Among them, timestamp represents the synchronization timestamp, time represents the sum of the call timestamp and the synchronization time difference, the type is double-precision floating point type, the unit is seconds, frequency represents the preset frequency, the type is integer type, the unit is Hz, the value of 1e-6 is 0.000001, and uint64_t represents the forced conversion of double-precision floating point type to integer type.

7. The time synchronization device of the multi-sensor fusion positioning system according to claim 6, characterized in that: The time difference calculation module is used for: If there is a synchronization time difference, the difference between the synchronization time difference and the time difference between the standard timestamp and the receiving timestamp of the IMU data in the circular queue is calculated in sequence; If the absolute value of the difference is greater than the deviation threshold, the corresponding IMU data is taken as a mutation frame, otherwise the corresponding IMU data is taken as a valid frame; If the maximum number of consecutive frames of mutation frames in the circular queue is less than N, and the interval time from the last calculation of the synchronization time difference reaches the update period, then the average time difference between the standard timestamp and the received timestamp of all valid frames in the circular queue is calculated to obtain a new synchronization time difference, where N is a positive integer greater than 1.

8. The time synchronization device of the multi-sensor fusion positioning system according to claim 7, characterized in that: The time difference calculation module is also used for: If N consecutive mutation frames are detected, all IMU data before these N mutation frames are removed from the circular queue, and the average time difference between the standard timestamp and the received timestamp of all remaining IMU data is calculated to obtain a new synchronization time difference.

9. A time synchronization device for a multi-sensor fusion positioning system, characterized in that: The time synchronization device of the multi-sensor fusion positioning system includes a processor, a memory, and a time synchronization program stored in the memory and executable by the processor, wherein when the time synchronization program is executed by the processor, the steps of the time synchronization method of the multi-sensor fusion positioning system as described in any one of claims 1 to 5 are implemented.

Images