A multi-sensor asynchronous data fusion method based on data interpolation

Abstract

The present invention discloses a multi-sensor asynchronous data fusion processing method based on data interpolation, and designs a positioning framework including a track calculation module, a data buffer module, a time index module, and a data interpolation module. The track calculation module performs posture estimation based on the vehicle's motion model. After the sensor obtains the measurement information, it is cached through the data buffer module. The time index module is used to achieve accurate matching of the timestamps of multi-sensor measurement information, and the data interpolation module is used to obtain the most accurate vehicle position information possible through the state estimator. The effectiveness of this method is verified by building an unmanned driving simulation platform. Experimental results show that the positioning algorithm implemented according to the positioning framework proposed in the present invention has the advantage of high precision.

Description

Technical Field

[0001] The present invention specifically relates to a multi-sensor asynchronous data fusion processing method based on data interpolation, and belongs to the field of multi-sensor fusion. Background Art

[0002] Driven by societal development needs and modern technological advancements, autonomous driving technology for smart vehicles has garnered widespread attention from the academic, research, and engineering communities. Various multi-sensor information fusion systems for complex applications are being researched and applied. In intelligent transportation scenarios, autonomous positioning is the core foundational technology for autonomous operation. To achieve good positioning results, relying solely on information provided by a single sensor is no longer sufficient. Multi-sensor data fusion positioning technology has become a key research direction in the smart vehicle field. This requires the use of multi-sensor information fusion systems, including radar and inertial measurement units (IMUs), to obtain diverse observation data and, through fusion processing, provide vehicle position information, thereby completing vehicle positioning.

[0003] However, in the actual operation of multi-sensor information fusion systems, both the system itself and the sensor's operating environment can cause time asynchrony in the observed data. These asynchronies can be both deterministic and random, arising from both the measurement process and the data transmission process. Fusion of multi-sensor data without time registration can result in significant fusion errors, impacting the overall performance of the multi-sensor system.

[0004] To address these shortcomings, temporal registration of sampled data has become a hot topic of research. Within temporal registration technology, the mismatch of sampled data caused by different sampling periods has been extensively studied, and several effective methods have been developed. These include interpolation and extrapolation, least squares virtualization, interpolation, curve fitting, and serial merging. In practical applications, interpolation and extrapolation and least squares virtualization are more commonly used. However, these two methods assume a uniform linear motion model for the target within the algorithm processing interval, making them more suitable for situations where the target velocity is constant or slowly changing.

[0005] Therefore, the present invention proposes a multi-sensor asynchronous data fusion processing method based on data interpolation, which combines time synchronization with data fusion and is an improved positioning algorithm. Compared with the traditional filtering algorithm, the biggest difference of the fusion method proposed in the present invention is that the multi-sensor asynchronous data fusion processing method based on data interpolation can accurately match multi-sensor data with the same timestamp, instead of adopting the traditional timestamp similarity strategy in ROS. The advantage of this fusion processing method is that it can accurately match the timestamps of multi-sensor data, improve the overall fusion effect, and thus improve the accuracy of vehicle autonomous positioning. Summary of the Invention

[0006] This paper proposes a multi-sensor asynchronous data fusion processing method based on data interpolation. Under asynchronous sensor sampling conditions, this method uses time indexing and data interpolation to obtain data from other sensors that match the same timestamps as accurately as possible. This method improves existing multi-sensor data fusion algorithms to enhance the accuracy of vehicle autonomous positioning.

[0007] This invention primarily includes a dead reckoning module, a time index module, a data interpolation module, and a data fusion module. In this invention, we assume that the noise of each observation is independent of each other. At each sampling moment, the data interpolation module can access data from the dead reckoning module and the data buffer module to interpolate the sampled data. The specific implementation steps are as follows:

[0008] Step 1: Perform dead reckoning based on the prior estimated state

[0009] Assume that the working area of ​​the smart car is an ideal horizontal two-dimensional environment, the state vector of the system is the position of the smart car, and the position of the smart car at time k is x k =(X k ,Y k ,θ k ) T where X k ,Y k represents the position of the geometric center of the smart car, θ k Represents the posture of the smart car in the navigation coordinate system. The posture transformation matrix of the smart car from the airborne coordinate system to the navigation coordinate system is:

[0010]

[0011] In order to gradually estimate the position and posture of the mobile robot, in the EKF, the data of the wheel odometer is used for state prediction, and the posture data provided by the IMU is used for measurement update.

[0012] Here, a wheel odometer is used to provide the control amount u for the prediction step. k+1 =(V Odo,X ,V Odo,Y ,ω Odo ), the motion model of the smart car (i.e., the pose parameter update equation) is:

[0013]

[0014] The estimated pose value at time k+1 predicted by the motion model is:

[0015]

[0016] In the formula represents the pose estimate at time k+1 predicted by the motion model. The symbol ^ represents the estimated value, and the symbol - represents the predicted value.

[0017] The covariance matrix of the prior estimate of the predicted state vector is:

[0018]

[0019] The Jacobian matrix of the motion model and the Jacobian matrix of motion noise are:

[0020]

[0021]

[0022] When measuring with a wheel odometer, the typical value of motion noise is w x =w y =0.1m / m,w θ =1° / m, so

[0023]

[0024] Step 2: Time indexing based on data timestamp

[0025] In a multi-sensor system, sensor data from different sensor systems is time-aligned relative to a reference time base. A reference time value can be communicated to the sensor systems so that they can tag sensor data based on the reference time value. Sensor data from a sensor system can be time-aligned by applying a calibration strategy to the sensor data.

[0026] The data cache module stores sensor data in separate containers in chronological order. Each container is equivalent to constructing a timeline. First, we need to select a core sensor (the odometer sensor) and use the core sensor's data timestamp as the reference time. Next, we perform time indexing on the data of other sensors. This means finding the corresponding position of the odometer sensor's acquisition time in the timeline of other sensors and locking the data of the previous and next frames to continue data interpolation.

[0027] When traversing the data in a non-core sensor data container, the ideal situation is to have the timestamp of the first data in the container be earlier than the reference time, and the timestamp of the second data be later than the reference time. Of course, some anomalies need to be considered when time-indexing non-core sensor data. If non-core sensor data experiences frame loss or timestamp anomalies, it may cause larger errors and even affect program functionality. Therefore, in order to ensure that the indexed data is correct, we also need to set constraints for it:

[0028] Condition 1: If the timestamp of the first data in the container is later than the reference time, then if data interpolation is performed here, the data before the reference time cannot be obtained, that is, it cannot be inserted. The traversal process should be exited and the timestamp of the next data moment of the core sensor should be selected as the reference time.

[0029] Condition 2: If the timestamp of the first data in the container is earlier than the reference time, the timestamp of the second data in the container must be later than the reference time. Otherwise, the first data is meaningless and the traversal should continue and the first data should be deleted.

[0030] Condition 3: If conditions 1 and 2 are met, but the difference between the timestamp of the first data and the reference time is greater than the threshold we set, it proves that the data may have been lost at that moment. The traversal process should be exited and the timestamp of the next data moment of the core sensor should be selected as the reference time.

[0031] Condition 4: If conditions 1, 2, and 3 are met, but the difference between the timestamp of the second data and the reference time is greater than the threshold we set, it proves that the data may have been lost at that moment. The traversal process should be exited and the timestamp of the next data moment of the core sensor should be selected as the reference time.

[0032] If the traversal process can simultaneously meet the above four constraints, the time index can be considered completed and the position should be locked for data interpolation.

[0033] Step 3: Interpolate data based on adjacent frame data

[0034] After completing the time index and locking the position in step 4, the data interpolation module uses the quaternion attitude interpolation method based on spherical vectors to achieve smooth attitude interpolation using the IMU data of the two frames before and after the position.

[0035] Assume that the quaternion q of the transition between the initial posture and the final posture is m , which can be represented by two rotation vectors perpendicular to the transition rotation axis on the unit sphere Indicates that these two rotation vectors are perpendicular to the axis of rotation The included angle is the rotation angle θ. When one vector rotates to another vector, an arc is formed on the unit sphere. These two vectors and the arc represent the quaternion q of the transition rotation. m Evenly distribute the points on the arc, and you can get a series of transition rotation four elements evenly distributed. After superimposing with the initial posture, a series of uniform interpolated postures are obtained

[0036] The specific steps of quaternion attitude interpolation are given below.

[0037] Step 1: Obtain the axis vector and angle of transition rotation from the given initial and final postures. The relevant equation is q m =q s -1 ·q e .

[0038] Step 2: Based on the parametric equation of the space arc, take a series of uniform points on the rotating arc. The space arc on the unit sphere can be expressed as the following equation.

[0039]

[0040] in is the axis vector of transition rotation, θ is the angle of rotation, and the normal vector of the plane where the arc is located is When the initial and final poses are given, these values ​​are fixed. t is the spatial arc parameter. By uniformly selecting the parameter t, a series of uniform points on the arc can be obtained.

[0041] Step 3: The arc starting point vector is Select points evenly from the starting point to the end point of the arc to obtain a series of vectors Quaternion q for transition rotation m Homogenization, that is

[0042]

[0043] And the quaternion of the axis of rotation orthogonal to the vector format into a non-orthogonal common format.

[0044] Step 4: Get the final series of quaternion attitude interpolation, namely:

[0045]

[0046] Step 4: Build an extended Kalman filter for data fusion

[0047] In the intelligent vehicle network, each smart car is equipped with an inertial measurement unit (IMU). We use a multi-sensor module to obtain the original state observation information of the on-board sensors as the state observation space. The attitude measurement information θ of the IMU at time k+1 is k+1,IMU , then the observation model is:

[0048] Z k+1 =θ k+1 +v k+1 (11)

[0049] At each moment, the inertial measurement unit and the odometer return complex observations of the same parameter (attitude angle). For this situation, we use the Kalman gain to calculate a confidence value for the uncertainty of the two sensor observations. The Kalman gain at time k+1 is:

[0050]

[0051] in Is a unit matrix, which is the covariance matrix of the observation noise. Generally, the manufacturer gives the accuracy or performs noise evaluation through experimental statistics. Here we assume that

[0052] R k+1 =σ θ (13)

[0053] where σ θ It represents the observation noise variance of the attitude output by the IMU.

[0054] so:

[0055]

[0056] The posterior estimate of the state variable is:

[0057]

[0058] The covariance matrix of the posterior estimate of the state variables is:

[0059] BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 The flowchart of the multi-sensor asynchronous data fusion processing method based on data interpolation.

[0061] Figure 2 Kinematic model of the mobile robot turtlebot3.

[0062] Figure 3 This is the structural diagram of the unmanned driving simulation platform.

[0063] Figure 4 This is the trajectory diagram of the fusion algorithm without adopting the time synchronization strategy.

[0064] Figure 5 The trajectory diagram of the fusion algorithm adopting different time synchronization strategies.

[0065] Figure 6 The comparison chart of yaw angle changes in each control group. DETAILED DESCRIPTION

[0066] The multi-sensor asynchronous data fusion processing method based on data interpolation of the present invention will be further described in detail below with reference to the accompanying drawings.

[0067] The algorithm model of the present invention is based on certain assumptions, including:

[0068] Assumption 1: All controlled vehicles are intelligent vehicles with sensing, computing, control, and communication capabilities.

[0069] Assumption 2: Assume that the observations of vehicle sensors are independent of each other;

[0070] Assumption 3: Assume that the noise of each observation follows a Gaussian distribution;

[0071] In this example, a mobile robot simulates vehicle driving behavior indoors, relying solely on onboard sensors to obtain vehicle position information. By setting up an experimental control group and performing relevant programming calculations with Matlab software, the performance of the mobile robot's positioning system was verified.

[0072] Step 1: Building the experimental platform

[0073] In order to simulate the normal driving scenario of vehicles in the intelligent vehicle network, this example uses the Turtlebot3 car to build an unmanned driving simulation platform. Turtlebot3 is the third generation of the TurtleBot series. It is driven by the robot intelligent driver Dynamixel. It is a small, programmable, cost-effective mobile robot based on ROS. Its kinematic model is as follows Figure 2 As shown in the figure, the Turtlebot3 robot is equipped with an inertial measurement unit (IMU) and an odometer (Odom). Through the built-in system function package, it can obtain real-time status information such as vehicle speed, angular velocity, and acceleration. The IMU operates at a frequency of 100 Hz, and the Odom operates at a frequency of 10 Hz. Based on the vehicle's differential wheel kinematic model, this example uses the following secant model as the mobile robot's kinematic equations:

[0074]

[0075] Among them, k is the current timestamp, x, y are the coordinates of the current vehicle, θ is the heading angle of the current vehicle, and Δt is the time interval between two adjacent moments.

[0076] Step 2: Vehicle state estimation

[0077] The unmanned driving platform relies on the ROS system and completes real-time monitoring of the system environment and driving behavior control by writing corresponding function packages. The platform structure is as follows: Figure 3 shown.

[0078] As mentioned above, this example uses vehicle position and vehicle posture as vehicle state variables, so the estimated vector X = [xyθ] T .

[0079] In order to obtain the noise information of each sensor, before the experiment started, the vehicle was placed in a stationary field, and samples were taken for a long time at equal intervals. The data were calculated and analyzed, and the results of the vehicle state quantities were obtained as shown in Table 1.

[0080] Table 1 Noise parameters

[0081]

[0082] When using an odometer to obtain velocity information while the vehicle is stationary, the odometer has a fixed noise level of approximately 5 x 10⁻⁴ m / s. If the vehicle is moving at a constant speed of 0.1 m / s, the standard deviation of the velocity observation error can be measured to be approximately 3 x 10⁻⁴ m / s. This deviation is used as the standard deviation of the noise when estimating the vehicle's position.

[0083] Step 3: Positioning accuracy test

[0084] In order to verify the positioning accuracy of the wheel odometer and IMU fusion positioning algorithm, 8 sampling target points a(2.000,0.000), b(2.000,1.000), c(0.000,1.000), d(0.000,2.000), e(2.000,2.000), f(2.000,3.000), g(1.000,3.000), h(1.000,1.000) are released in the mobile robot ROS system. The smart car passes through these eight target points in sequence from the origin of the coordinate system. The motion trajectory between each target point is autonomously planned by the navigation algorithm. The positioning accuracy of the target point can be used to estimate the positioning accuracy of the motion process. The trajectory of each positioning method is as follows: Figure 4 、 Figure 5 As shown, Figure 4 In the figure, the control group ① is the trajectory of the fusion localization algorithm without time synchronization strategy; the odom trajectory is the trajectory of the wheel odometry. The black trajectory is the expected trajectory. Figure 5 In the example, control group ② is the trajectory of the fusion positioning algorithm using a time synchronization strategy with similar timestamps, while control group ③ is the trajectory of the fusion positioning algorithm using a time synchronization strategy with data interpolation. The specific coordinate values ​​of each point in the different trajectories are shown in Table 2. The positioning accuracy of the three time synchronization strategies is calculated using standard deviation, and the accuracy values ​​are shown in Table 3.

[0085] Table 2 Coordinate values ​​of each point in different trajectories

[0086]

[0087] Table 3 Accuracy of sampling coordinate points under various time synchronization strategies

[0088]

[0089] Step 4: Yaw angle change test

[0090] In order to verify the effectiveness of the present invention for attitude estimation, the experimental data of the three control groups in step 3 are recorded, and the yaw angle change comparison curve is drawn. The results are as follows: Figure 6 The yaw angle values ​​for each control group are shown in Table 3. It can be seen that during the initial stage of vehicle operation, i.e., the process of approaching point 1, there was no change in attitude angle, so the positioning results of the three control groups were similar. Subsequently, during the vehicle's steering process, the attitude angle changed. Control group ① continuously accumulated errors, and at point 3, the yaw angle error exceeded 10°. As the vehicle continued to move, the yaw angle of control group ① gradually lost its meaning. However, the yaw angle data of control groups ② and ③ both accurately reflected the actual changes (the instantaneous jump in the figure is caused by the slight change between -180° and 180°). This demonstrates the effectiveness of the quaternion attitude interpolation method based on spherical vectors for attitude fusion.

[0091] Table 3 Yaw angle values ​​at each point in different trajectories

[0092]

[0093] Step 5. Experimental results

[0094] This paper proposes a multi-sensor data fusion method based on data interpolation to address the problem of asynchronous sensor data fusion in vehicles. By combining data interpolation with pose estimation, we ensure that the data required by the multi-sensor fusion algorithm and the data in the buffer are at matching times. The effectiveness of the proposed data interpolation method was verified using an experimental platform. The experimental results demonstrate the high accuracy of the positioning algorithm implemented using the proposed positioning framework.

Claims

1. A multi-sensor asynchronous data fusion processing method based on data interpolation, characterized by: It includes a dead reckoning module, a data buffer module, a time index module, and a data interpolation module. The dead reckoning module estimates the position and posture of the vehicle based on its motion model and inputs it into the state estimator as a priori estimation. The data buffer module is responsible for receiving the measurement information of each vehicle sensor at the current moment and storing it in the corresponding data buffer container in chronological order. The time index module traverses the sensor data in the data buffer container according to the reference time and locks the position. The data interpolation module uses the frame data before and after the locked position to interpolate the sensor data and inputs the interpolated data as observation information into the state estimator to achieve a posteriori estimation of the vehicle position. The dead reckoning module is a hybrid extended Kalman filter structure. According to the dynamic model of each vehicle, the extended Kalman filter is used to estimate the state of the vehicle. The equation is as follows: Among them, x k =[X k ,Y k ,θ k ] T represents the vehicle's position at time k, X k ,Y k represents the position of the vehicle, θ k Represents the vehicle's posture in the navigation coordinate system; represents the pose estimate at time k+1 predicted by the motion model; u k+1 =[V Odo,X ,V Odo,Y ,w Odo ] T Indicates the control value provided by the wheel odometer for the prediction step; the symbol "^" represents the estimated value, and the symbol "-" represents the predicted value; Among them, P k and Q k+1 They are posture x k and process noise w k+1 The covariance matrix of and are the Jacobian matrices of the motion model and the Jacobian matrix of the process noise, respectively; in, Represents the Jacobian matrix of the observation model, R k+1 represents the covariance matrix of the observation noise; R k+1 =s θ (5) Among them, σ θ Represents the observation noise variance of the attitude output by the IMU; At time k, equation (2) estimates the state through the vehicle's kinematic model and posture transformation matrix to obtain the prior prediction value at time k+1 Equation (3) is used to calculate the covariance matrix of the prior estimate of the state estimator at time k+1, and Equation (4) is used to calculate the error covariance matrix of the prior prediction value and the observation error covariance matrix R k+1 , get the Kalman gain K at time k+1 k+1 ; Equation (5) assumes that the sensor's observation noise variance R k+1 ; Equation (6) is obtained by observing the quantity Z k+1 and prior predictions Get the estimated state of the vehicle at time k+1 Equation (7) is obtained by the Kalman gain K at time k+1 k+1 and the error covariance matrix of the prior prediction value Get the estimated error covariance matrix P at time k+1 k+1 ; The data interpolation module selects the corresponding interpolation method based on the two frames of data before and after the position locked by the time index module to achieve accurate temporal matching of multi-sensor data. For the sensor data of the inertial measurement unit, the quaternion attitude interpolation method based on spherical vectors is selected for data interpolation. The interpolation equation is as follows: Wherein, equation (8) is given by the initial posture q s and end gesture q e To obtain the transition rotation q m ; A series of points are uniformly selected on the space arc on the unit sphere given by equation (9) according to the uniform selection parameter t; equation (10) is obtained by uniformly selecting the points Quaternion q for transition rotation m Homogenization; Equation (11) is obtained by initial posture q s and transition rotation q m To get a series of interpolated quaternion poses.

2. The multi-sensor asynchronous data fusion processing method based on data interpolation according to claim 1, characterized in that: The IMU sensor data with timestamps is stored in a data buffer container. The odometer sensor is selected as the core sensor. The odometer data timestamp is used as the reference time. The observation data in the data buffer container, i.e., the IMU information, is traversed to achieve accurate temporal matching of the sensor data. The data types of the dead reckoning module's estimated state include position coordinates and yaw angles. The data types of the dead reckoning module include vehicle driving data and vehicle state information measured by onboard sensors. The vehicle observation model is obtained as follows: Z k+1 =θ k+1 +v k+1 (1) Among them, Z k+1 represents the system observation at time k+1, θ k+1 Represents the attitude vector information of IMU, v k+1 represents the observation noise at time k+1.

3. The multi-sensor asynchronous data fusion processing method based on data interpolation according to claim 2, characterized in that: The dead reckoning module is a hybrid extended Kalman filter structure. The process of implementing dead reckoning is as follows: S1. Establish the corresponding state space equation and observation equation according to the vehicle's dynamic model and make a priori predictions; S2. Perform time indexing based on the data timestamp of the measurement information; S3, perform data interpolation based on adjacent frame data to process system observation quantity Z k+1 ; S4. Based on the prior prediction value obtained in S2 and the system observation value obtained in S3, an extended Kalman filter is constructed to estimate the vehicle position and posture.

4. The multi-sensor asynchronous data fusion processing method based on data interpolation according to claim 1, characterized in that: The data buffer module stores the sensor data in separate containers in chronological order. The time index module uses the timestamp of the core sensor data as a reference time and traverses the containers storing other sensor data until it finds the corresponding location. The traversal rules are as follows: Condition 1: If the timestamp of the first data in the container is later than the reference time, then if data interpolation is performed here, the data before the reference time cannot be obtained, that is, no insertion is possible. The traversal process is aborted, and the timestamp of the next data of the core sensor is selected as the reference time. Condition 2: If the timestamp of the first data in the container is earlier than the reference time, the timestamp of the second data in the container must be later than the reference time. Otherwise, the first data is meaningless and the traversal should continue and the first data should be deleted. Condition 3: If conditions 1 and 2 are met, but the difference between the timestamp of the first data and the reference time is greater than the set threshold, it indicates that there may be data frame loss at that moment. The traversal process is exited and the timestamp of the next data of the core sensor is selected as the reference time. Condition 4: If conditions 1, 2, and 3 are met, but the difference between the timestamp of the second data and the reference time is greater than the set threshold, it indicates that data may have been lost at that moment. The traversal process is exited, and the timestamp of the next data from the core sensor is selected as the reference time. If the traversal process can simultaneously meet the above four constraints, the time index can be considered completed, and the position should be locked for data interpolation.

Images