Hardware time synchronization method based on ROS outdoor integrated navigation

Abstract

The invention discloses a hardware time synchronization method based on ROS outdoor integrated navigation, and the method comprises the following steps: S1, calibrating IMU parameters, and setting a GPS and the IMU to be in a PPS output and trigger mode; s2, time is initialized, an internal crystal oscillator timer is started, timestamps are stamped on IMU data, and the data are synchronously stored in a USB flash disk; s3, detecting and improving the problems of signal loss and leap second; s4, the IMU data and the GPS data which are stamped with the timestamps are sent to an ROS node through the same serial port, and the ROS node analyzes the IMU data and the GPS data; s5, task priority and data priority processing is realized, a slice Kalman filtering algorithm is used for high-time-consumption Kalman filtering, and for the wheel angle sensor which cannot receive PPS pulses through an interface, ROS is used for realizing time synchronization; and S6, compensating the time delay of the control information. According to the invention, hard time synchronization in integrated navigation is realized, the reliability is high, and the expandability is strong.

Description

Technical Field

[0001] This invention belongs to the field of intelligent agricultural machinery technology, specifically relating to a hardware time synchronization method based on ROS outdoor integrated navigation. Background Technology

[0002] Since its inception, ROS (robotics operating system) has become the best choice for universities and research institutions to conduct research and verify algorithms, thanks to its distributed architecture, multi-language support, and strong open-source sharing community. GNSS / INS integrated navigation is one of the key technologies for vehicle positioning and navigation.

[0003] One significant challenge in developing integrated navigation systems within ROS for practical applications is ensuring the time synchronization of various sensors and mitigating internal latency. High reliance on networks, an inherently non-real-time design, variations in sensor models, data transmission delays, and unpredictable latency during operating system scheduling all contribute to the difficulty in guaranteeing the accuracy of GNSS / INS integrated navigation under ROS. Common time synchronization solutions include hardware time synchronization based on GPS / RTK, software time synchronization based on the high-precision time synchronization protocol PTP, and filtering based on ROS's built-in message filters. PTP software synchronization requires the addition of a network switch supporting the PTP protocol. Hardware time synchronization based solely on GPS / RTK is highly dependent on GPS signals and struggles to handle short-term signal loss. ROS's built-in filtering methods are only effective for signals of the same frequency; otherwise, high-frequency information is dragged down to lower frequencies, resulting in significant data loss. Summary of the Invention

[0004] The main objective of this invention is to overcome the shortcomings and deficiencies of the prior art and propose a hardware time synchronization method based on ROS outdoor integrated navigation. Based on the PPS pulses of STM32 and GPS devices, it adopts a lower-level computer synchronization and upper-level computer calculation method, which can achieve high-quality time synchronization effect on ordinary laptops.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A hardware time synchronization method based on ROS outdoor integrated navigation includes the following steps:

[0007] S1, IMU parameter calibration, setting GPS and IMU to PPS output and trigger mode;

[0008] S2. Initialize the time, start the internal crystal oscillator timer, timestamp the IMU data, and simultaneously save the data to the USB flash drive;

[0009] S3. Detection and improvement of signal loss and leap second issues;

[0010] S4. The timestamped IMU data and GPS data are sent to the ROS node via the same serial port for parsing.

[0011] S5. Implement task priority and data priority processing, use sliced ​​Kalman filter algorithm for time-consuming Kalman filtering, and use ROS to achieve time synchronization for wheel angle sensors that cannot receive PPS pulses through the interface.

[0012] S6. Compensate for the time delay of control information.

[0013] The present invention also includes a computer-readable storage medium storing a program that, when executed by a processor, implements the hardware time synchronization method for ROS-based outdoor integrated navigation provided by the present invention.

[0014] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0015] 1. This invention achieves hard time synchronization in integrated navigation in a low-cost manner, with high reliability and strong scalability. It allows for easy addition of other sensors for time synchronization and has no requirements on sensor brands. For ROS1, which is still widely used, its underlying design limits real-time performance, resulting in significant improvements to problems such as severe latency and reconnection after network loss. It can reduce the time cost of migrating to ROS2 to a certain extent, so that experiments for scientific research purposes can be less affected. Attached Figure Description

[0016] Figure 1 This is a flowchart of an embodiment of the present invention;

[0017] Figure 2 This is a hardware connection diagram in an embodiment of the present invention;

[0018] Figure 3 This is a hardware connection schematic diagram in an embodiment of the present invention;

[0019] Figure 4 This is a sensor pulse diagram from an embodiment of the present invention;

[0020] Figure 5 This is the sensor pulse diagram of the present invention. Detailed Implementation

[0021] This invention discloses a hardware time synchronization method based on ROS outdoor integrated navigation, comprising the following steps:

[0022] S1, IMU parameter calibration, setting GPS and IMU to PPS output and trigger mode; specifically:

[0023] Allen analysis and ellipsoid fitting were used to calibrate the 9-axis IMU for acceleration zero bias, angular velocity zero bias, acceleration random walk, angular velocity random walk, and the soft and hard magnetic interference matrix of the magnetometer.

[0024] Configure the GPS to output via serial port and PPS pulse, and configure the IMU to use rising edge trigger mode and serial port output mode.

[0025] S2. Initialize the time, start the internal crystal oscillator timer, timestamp the IMU data, and synchronously save the data to the USB drive; specifically:

[0026] After power-on, the system sequentially performs light detection and buzzer detection. When the first PPS arrives, it obtains the required integer seconds via UTC time, synchronously starts general-purpose timer 2, uses the MCU's internal crystal oscillator timer to achieve millisecond-level time increments, marks the complete timestamp on the IMU data, transmits it through the serial port, and synchronously saves it to the storage device.

[0027] S3. Detection and improvement measures are implemented to address signal loss and leap second issues; specifically:

[0028] Each time a PPS pulse signal arrives, the range between the integer seconds from the last available PPS and the current integer seconds of UTC time is checked in real time to see if they are within 1 second. If they are, it means that the signal has not been lost during this period; if they are not, there is a signal loss, and the latest integer seconds of UTC time are used to replace it.

[0029] To prevent potential leap seconds, the firmware library is updated manually on a regular basis.

[0030] S4. The timestamped IMU data and GPS data are sent to the ROS node via the same serial port for parsing; specifically:

[0031] Different transmission strategies are used for GPS and IMU data. For GPS serial data, only UTC time correction and initialization are required. After ensuring that the PPS signal is updated to integer seconds without any problems, the data is put into the buffer of the transmitting serial port and then sent. For IMU data, the data is first parsed in the MCU, and the pulse trigger time and transmission time are taken into account before the accurate timestamp is calculated.

[0032] S5. Implement task priority and data priority processing; use the sliced ​​Kalman filter algorithm for time-consuming Kalman filtering; and use ROS for time synchronization for wheel angle sensors that cannot receive PPS pulses through the interface. Specifically:

[0033] For multi-sensor fusion scenarios such as integrated navigation, program design is optimized, including but not limited to using multi-threading to process information from each sensor, using the sliced ​​Kalman filter algorithm to divide computing power in the Kalman filter used in integrated navigation, and using ROS to achieve time synchronization for wheel angle sensors that cannot receive PPS pulses through the interface.

[0034] S6. Compensate for the time delay of control information; specifically:

[0035] The control delay is obtained by the following formula, which further aligns the time of the host computer with that of the PPS.

[0036] Δt=ROS::Time::now()-t imu

[0037] After obtaining the control delay, a buffer is used to store historical data, and interpolation is used to estimate the accurate control information at the current moment.

[0038] The present invention will be further described in detail below with reference to the embodiments and accompanying drawings, but the embodiments of the present invention are not limited thereto.

[0039] Example

[0040] A hardware time synchronization method based on ROS outdoor integrated navigation, such as Figure 1 As shown, it includes the following steps:

[0041] S1. IMU parameter calibration: Setting the GPS and IMU to PPS output and trigger mode. In this embodiment, this specifically includes:

[0042] The acceleration zero bias, angular velocity zero bias, acceleration random walk, angular velocity random walk, and soft and hard magnetic interference matrix of the 9-axis IMU were calibrated using Allen analysis and ellipsoid fitting method.

[0043] Connect all sensors' serial ports, trigger lines, pulse lines, and USB flash drive data acquisition devices using an STM32 microcontroller. Ensure the PPS triggers correctly, the IMU is in trigger mode, and the trigger delay time can be obtained (calculated using the Rdy signal or simple transmission delay time). Figure 2 and Figure 3 The figures shown are the hardware connection diagram and schematic diagram for this embodiment. Ensure the PPS output is normal and the IMU trigger port can emit pulses correctly; the pulse diagrams for both are shown below. Figure 4 and Figure 5 As shown.

[0044] For an IMU with a baud rate of 115200bps (RS232), transmitting 100Hz quaternion data (16 bytes + 12 bits of acceleration + 12 bits of angular velocity + 12 bits of magnetometer = 52 bits of data, where the 5 in the numerator refers to the data length of "header + BID + MID + length flag + checksum").

[0045]

[0046] It is evident that at a frequency of 100Hz (10ms), this transmission delay is already quite significant. If it is not eliminated, it will cause subsequent control delays.

[0047] S2. Initialize the time by timestamping the IMU data using GPS time and the microcontroller crystal oscillator. In this embodiment, the specific steps are as follows:

[0048] When the first PPS pulse signal arrives, the STM32 microcontroller's timer is activated. Since the serial port transmission time is longer than the direct GPIO interrupt, the UTC time will arrive slightly later from the GPS serial port, and its integer seconds will be extracted as the initial time t. 整数秒 The crystal oscillator timer is turned on, and the PD7 level toggles once for each count, ensuring a toggle level of one millisecond, denoted as t. 小数秒 ;like Figure 2 As shown. Before the next PPS signal arrives, the IMU is triggered by the rising edge of the crystal oscillator timer. However, this time is only the trigger time, not the actual IMU data time. The data transmission time t must also be added. 延迟 That is the final IMU timestamp.

[0049] S3. To detect and improve the signal loss problem, in this embodiment, the specific steps are as follows:

[0050] GPS signals can be obstructed or interfered with in some areas, particularly in the smart agricultural machinery industry. Signal loss occurs when machinery is moved or at the boundaries of farmland with poor signal, often followed by a period of signal loss before the signal returns. Based on this, the following approach was designed: When each PPS pulse signal arrives, the signal is first compared to the previous pulse (t). 整数秒 If the 1s increment is within the 1s UTC time from the serial port, and is within the 1s range, it indicates that there is no PPS loss caused by signal loss. Therefore, t... 整数秒 If the value exceeds 1 second, it indicates that a signal has been lost. The latest UTC time needs to be updated to the integer second. This process is repeated continuously. During the period of signal loss, the timer is not reset, and the timestamp of the crystal oscillator timer is always trusted.

[0051] S4. The timestamped IMU data and GPS data are sent to the ROS node via the same serial port for parsing; in this embodiment, this specifically includes:

[0052] S41. Determine the data output strategy for serial port 5, specifically:

[0053] Serial port 5 maintains DMA transfer mode. GPS data is only used to obtain UTC time. The strategy is to send out one data packet as soon as it arrives on serial port 2. IMU data arriving on serial port 1 needs to be parsed first, a timestamp is added, and then it is placed in the buffer. One data packet is sent at a time, which can effectively avoid the situation where data arrives at the serial port at the same time and data is missed.

[0054] t imu时间戳＝ t 整数秒 +t 小数秒

[0055] The S42 and ROS nodes parse serial port 5 data as follows:

[0056] The strategy of "lower-level machine synchronization and high-performance computing" can complement each other's advantages while maintaining a high degree of decoupling. In ROS, data from the serial port is encapsulated into topics through a publish-subscribe mechanism. In terms of time, the method of obtaining local time using ros::Time::now() is abandoned, and the timestamps provided by the sensors are used instead, that is, the timestamps in header.headr.stamp are replaced by the timestamps of the sensors.

[0057] S5. Implement task priority and data priority processing; use the sliced ​​Kalman filter algorithm for time-consuming Kalman filtering; and use ROS to achieve time synchronization for wheel angle sensors that cannot receive PPS pulses through the interface. In this embodiment, it includes:

[0058] S51. Optimize data processing and task scheduling strategies, and implement a slice Kalman design for the time-consuming integrated navigation algorithm; specifically:

[0059] ROS's underlying architecture dictates that it is merely an intermediate suite built on non-real-time systems, and its performance in data processing and task scheduling is not ideal for fast-response scenarios. Critical data processing, such as GPS data updates and parsing, employs multi-threading and asynchronous processing. For time-consuming updates like Kalman filtering, a time-slice Kalman filtering method is used. The Kalman filtering update formula can be described as:

[0060] ① One-step state prediction:

[0061] ② Mean squared error of state one-step prediction:

[0062] ③ Filter gain:

[0063] ④ State estimation:

[0064] ⑤ Mean square error of state estimation: P k =(IK k H k )P k / k-1 ;

[0065] In strapdown inertial navigation systems, the time update frequency cannot be too low. For example, if the IMU receives data at 200 Hz, it needs to complete a Kalman time update within 5 ms, especially in ②, where 2n... 3 This multiplication will be more difficult. ② In the formula, phi*P*phi T The multiplication of two n-order matrices is the most computationally intensive part of the Kalman filter process. Multiplying an n*n matrix by an n*1-dimensional vector is considered as a single computational step (n... 2 (Multiplication by multiplication), then ① contains one part, ② contains 2n parts, ③ contains r parts (r sequential measurements); ⑤ contains r parts, for a total of 2(n+r) parts of calculation.

[0066] Distributing the 2(n+r) computational loads across different 5ms (time slices), and calculating only nSteps at a time, can significantly reduce computation time. Ensuring that the entire 2(n+r) / nStep*5ms < 50ms ensures that the Kalman filter can be updated.

[0067] The above approach minimizes time latency at the algorithm and program design levels, which can alleviate the shortcomings of ROS's poor real-time performance to some extent.

[0068] S52. For wheel angle sensors that cannot receive PPS pulses through the interface, ROS is used to achieve time synchronization, as detailed below:

[0069] In Ackerman-type ROS robots, real-time cornering requires feedback from wheel angle sensors, but most of these sensors cannot achieve a trigger mode for time synchronization within the MCU. Previously, time synchronization was not performed in this case; a rough approach was adopted where the code layer processed the data as soon as it was received. In this example, the transmission time is calculated to be 0.4ms using the following formula (84 bits transmitted at 250kbps).

[0070]

[0071] Compared to the time gap between the IMU's 200Hz and GPS's 10Hz, this is not a significant error. A timestamp can be added directly using `ros::Time::now()` when the data is received from the wheel angle sensor.

[0072] S6. Compensate for the time delay of the control information. In this embodiment, the specific steps are as follows:

[0073] Due to the time delay of ROS, such as in multi-axis transformation scenarios, problems such as untimely TF conversion are often encountered. To address these issues, one approach is to avoid using the built-in TF tool and write the conversion process in C++, which can effectively improve the situation. Another approach is to use a certain time delay control method. The time delay can be obtained in the following way, thereby further aligning the host computer's time with PPS.

[0074] Δt=ROS::time::now()-t imu

[0075] After obtaining the control delay, this example uses a buffer to store historical data and interpolation to estimate the accurate control information at the current moment.

[0076] In another embodiment, a computer-readable storage medium is provided, storing a program that, when executed by a processor, implements the hardware time synchronization method of the ROS-based outdoor integrated navigation system of the present invention, specifically as follows:

[0077] IMU parameter calibration, setting GPS and IMU to PPS output and trigger mode;

[0078] Initialize the time, start the internal crystal oscillator timer, timestamp the IMU data, and simultaneously save the data to the USB drive;

[0079] Detection and improvement measures were implemented to address signal loss and leap second issues.

[0080] The timestamped IMU data and GPS data are sent to the ROS node via the same serial port, where the node parses them.

[0081] Implement task priority and data priority processing, use sliced ​​Kalman filter algorithm for time-consuming Kalman filtering, and use ROS to achieve time synchronization for wheel angle sensors that cannot receive PPS pulses through the interface.

[0082] Compensation is provided for time delays in control information.

[0083] The computer-readable storage medium may be transient or non-transient. Exemplary examples include, but are not limited to, various media capable of storing computer program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0084] For example, the processor can be a central processing unit (CPU), a microprocessor unit (MPU), a digital signal processor (DSP), or a field programmable gate array (FPGA), etc.

[0085] It should also be noted that, in this specification, terms such as "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0086] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A hardware time synchronization method based on ROS outdoor integrated navigation, characterized in that, Comprise the following steps: S1, IMU parameter calibration, set GPS and IMU at PPS output and trigger mode; S2, initialization time, start internal crystal oscillator timer, give IMU data time stamp, and save data to U disk synchronously; S3, detection and improvement for signal loss and leap second problem; S4, send the IMU data and GPS data with time stamp to ROS node through the same serial port, and analyze by the node; S5, realize task priority and data priority processing, use slice Kalman filtering algorithm for high time-consuming Kalman filtering, use ROS to realize time synchronization for wheel angle sensor which cannot receive PPS pulse through interface; S6, compensate for control information time delay.

2. The hardware time synchronization method based on ROS outdoor integrated navigation according to claim 1, wherein, Step S1 is specifically: Allen analysis and ellipsoid fitting method is used to calibrate 9-axis IMU's acceleration zero offset, angular velocity zero offset, acceleration random walk, angular velocity random walk, and soft and hard magnetic interference matrix of magnetometer; Set GPS to serial output and PPS pulse output, and set IMU to rising trigger mode and serial output mode.

3. The hardware time synchronization method based on ROS outdoor integrated navigation according to claim 1, wherein, Step S2 is specifically: After power on, light detection and buzzer detection are performed in turn, and when the first PPS comes, the required integer seconds are obtained through UTC time, the general timer 2 is started synchronously, the internal crystal oscillator timer of MCU is used to realize millisecond level time increase, and the complete time stamp is marked on the IMU data and transmitted through the serial port, and is saved to the storage device synchronously.

4. The hardware time synchronization method based on ROS outdoor integrated navigation according to claim 1, wherein, Step S3 is specifically: When each PPS pulse signal comes, it is detected whether the integer seconds of the last available PPS and the integer seconds of the current UTC time are within 1s, if yes, it means that the signal has not been lost during this period; If not, the latest UTC time integer second is used to replace it; For the possible leap second phenomenon, manually update the firmware library periodically to avoid it.

5. The hardware time synchronization method based on ROS outdoor integrated navigation according to claim 1, wherein, Step S4 is specifically: Different transmission strategies are used for GPS and IMU data, that is, the data from GPS serial port is corrected and initialized by UTC time, and then put into the sending serial port buffer after ensuring that the PPS signal updates the integer seconds without problem; For IMU, the data is analyzed in MCU first, and then the accurate time stamp is calculated after considering the pulse trigger time and transmission time.

6. The hardware time synchronization method based on ROS outdoor integrated navigation according to claim 1, wherein, Step S5 is specifically: For the multi-sensor fusion scene of integrated navigation, optimization is carried out in program design, including but not limited to using multithreading to process each sensor information, using slice Kalman filtering algorithm to divide computing power in Kalman filtering used in integrated navigation; For wheel angle sensor which cannot receive PPS pulse through interface, use ROS to realize time synchronization.

7. The hardware time synchronization method based on ROS outdoor integrated navigation according to claim 1, wherein, Step S6 is specifically: The control delay is obtained by the following formula, so as to further align the time of host computer with PPS; Δt = ROS : : Time : : now() - t imu After obtaining the control delay, use the buffer to store historical data, and use interpolation method to estimate the accurate control information at the current time.

8. A computer-readable storage medium storing a program, characterized in that, The program is executed by the processor to implement the hardware time synchronization method based on ROS outdoor integrated navigation according to any one of claims 1-7.

Images