CN115577320A - Multi-sensor asynchronous data fusion method based on data interpolation - Google Patents

Abstract

The invention discloses a multi-sensor asynchronous data fusion processing method based on data interpolation, and designs a positioning frame comprising a dead reckoning module, a data buffering module, a time index module and a data interpolation module. And the track calculation module carries out pose estimation according to the motion dynamic model of the vehicle. And the sensor caches the measurement information through the data buffering module after acquiring the measurement information. The accurate matching of the multi-sensor measurement information time stamps is realized through the time index module, and the vehicle position information which is as accurate as possible is obtained through the data interpolation module and the state estimator. The effectiveness of the method is verified by building an unmanned simulation platform. Experimental results show that the positioning algorithm realized by the positioning frame provided by the invention has the advantage of high precision.

Description

A Multi-sensor Asynchronous Data Fusion Method Based on Data Interpolation

technical field

The 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 technique

With the traction of social development needs and the promotion of modern scientific and technological progress, intelligent vehicle automatic driving technology has received extensive attention from academic research and engineering circles, and various multi-sensor information fusion systems for complex application backgrounds have been continuously researched and applied. In the intelligent transportation scenario, the core basic technology for the autonomous operation of intelligent vehicles is autonomous positioning. In order to obtain a good positioning effect, relying on the information provided by a single sensor can no longer meet the positioning accuracy requirements. Multi-sensor data fusion positioning technology has become one of the important research directions in the field of smart vehicles. It must use radar, inertial measurement unit (IMU), etc. A multi-sensor information fusion system of various sensors is used to obtain a variety of observation data, and to provide vehicle position information through fusion processing, so as to complete the task of vehicle positioning.

However, in the actual work of the information fusion system of the multi-sensor system, the system itself and the working environment of the sensor are the reasons for the time asynchrony of the observed data, and among various reasons, there are certain and random, and some are caused by the measurement process. It is also caused by the data transmission process. If the multi-sensor data information is fused without time registration, large fusion errors will be generated, which will affect the overall performance of the multi-sensor system.

In order to make up for the above shortcomings, time registration of sampled data has gradually become a research hotspot. In the research of time registration technology, there are many studies on the problem of mismatching of sampling data caused by different sampling periods, and some effective methods have been obtained. There are mainly interpolation and extrapolation methods, least squares virtual method, interpolation method, curve fitting method and serial combination method. In practical applications, the interpolation and extrapolation method and the least squares virtual method are commonly used, but the target motion model adopted by these two methods in the algorithm processing time interval is uniform linear motion, which is more suitable for constant or slow change of target speed. Case.

Therefore, the present invention proposes a multi-sensor asynchronous data fusion processing method based on data interpolation, which combines time synchronization and data fusion, and is an improved positioning algorithm. Compared with the traditional filtering algorithm, the biggest difference of the fusion method proposed by the present invention is that the multi-sensor asynchronous data fusion processing method based on data interpolation can accurately match the multi-sensor data with the same time stamp, instead of using the traditional time in ros Poke similar strategies. The advantage of this fusion processing method is that it can accurately match the time stamps of multi-sensor data, improve the overall fusion effect, and thereby improve the accuracy of vehicle autonomous positioning.

Contents of the invention

The invention proposes a multi-sensor asynchronous data fusion processing method based on data interpolation. In the case of sensor asynchronous sampling time, other sensor data that match the same time stamp as accurately as possible can be obtained by time indexing and data interpolation on the original observation time stamp of the vehicle sensor. The invention improves the accuracy of vehicle autonomous positioning by improving the existing multi-sensor data fusion algorithm.

The invention mainly includes a dead reckoning module, a time index module, a data interpolation module and a data fusion module. In the present invention, we assume that the noise of each observed value is independent of each other. At each sampling moment, the data interpolation module can obtain the data in the dead reckoning module and the data buffer module to realize the interpolation of the sampled data. The specific implementation steps are as follows:

Step 1. Perform dead reckoning for the prior estimated state

Assuming that the working area of the smart car is an ideal horizontal two-dimensional environment, the state vector of the system is the pose of the smart car, and the pose of the smart car at time k is known as x _k = (X _k , Y _k , θ _k ) ^T where X _k , Y _k represent the position of the geometric center of the smart car, and θ _k represents the attitude of the smart car in the navigation coordinate system. The pose transformation matrix of the pose of the smart car from the airborne coordinate system to the navigation coordinate system is:

In order to gradually estimate the pose of the mobile robot, in the EKF, the data of the wheel odometer is used for state prediction, and the attitude data provided by the IMU is used for measurement update.

Here, the wheel odometer is used to provide the control quantity u _k+1 = (V _Odo,X ,V _Odo,Y ,ω _Odo ) for the prediction step, and the motion model of the smart car (that is, the pose parameter update equation) is:

According to the motion model, the estimated value of the pose at time k+1 is:

表示由运动模型预测的k+1时刻的位姿估计值。其中，符号^表示估计值，符号-表示预测值。In the formula

Indicates the pose estimate at time k+1 predicted by the motion model. Among them, the symbol ^ represents the estimated value, and the symbol - represents the predicted value.

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

The Jacobian matrix of the motion model and the Jacobian matrix of the motion noise are:

When measuring the wheel odometer, the typical value of motion noise is w _x ＝w _y ＝0.1m/m, w _θ ＝1°/m, so

Step 2. Perform time indexing based on the data timestamp

In a multi-sensor system, sensor data from different sensor systems are time-aligned with respect to a reference time reference. The reference time value may be communicated to the sensor system in order for the sensor system to tag sensor data based on said reference time value. Sensor data from a sensor system can be time-calibrated by applying a calibration strategy to the sensor data.

The data cache module stores sensor data independently in chronological order in their respective containers. Each container is equivalent to building a timeline. First, we need to select a core sensor (odometer sensor) and store the data time of the core sensor Stamp as reference time. Secondly, perform time indexing operation on the data of other sensors, that is, find the corresponding position in the timeline of other sensors at the acquisition time of the odometer sensor, and lock the data of the front and rear frames in order to continue the data interpolation process.

When traversing the data in the 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 in the container is later than the reference time. Of course, some abnormalities need to be considered when performing time indexing on non-core sensor data. If the non-core sensor data loses frames or the timestamp is abnormal, it may lead to larger errors and even affect program functions. Therefore, in order to ensure that the indexed data is correct, we also need to set restrictions for it:

Condition 1: If the timestamp of the first data in the container is later than the reference time, it means that if data interpolation is performed here, the data at the previous moment of the reference time cannot be obtained, that is, there is no way to insert, and the traversal process should be exited, select The data timestamp of the core sensor at the next moment is used as the reference time.

Condition 2: If the time stamp of the first data in the container is earlier than the reference time, the time stamp of the second data in the container must be later than the reference time, otherwise, the first data is meaningless , the traversal should continue and the first data should be deleted.

Condition 3: If condition 1 and condition 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 data frame loss at this moment, and the traversal process should be exited , select the data timestamp of the core sensor at the next moment as the reference time.

Condition 4: If condition 1, condition 2, and condition 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 there may be data frame loss at this moment in the data, and it should be Exit the traversal process, and select the data timestamp of the core sensor at the next moment as the reference time.

If the traversal process can meet the above four constraints at the same time, it can be considered that the time index is completed, and the position should be locked for data interpolation.

Step 3. Perform data interpolation based on adjacent frame data

Step 4 After completing the time index and locking the position, the data interpolation module uses the quaternion attitude interpolation method based on the spherical vector, and uses the IMU data of the two frames before and after the position to realize the smooth interpolation of the attitude.

表示，这两个旋转向量垂直于转轴

夹角为转角θ，一个向量旋转到另一个向量时，在单位球面上形成一段圆弧，这两个向量及圆弧就表示过渡转动的四元数q_m。均匀分配圆弧上的点，则可以得到均匀分配得到一系列过渡转动四元素

与初始姿态叠加之后，得到一系列均匀的插值姿态

Assuming the quaternion q _m of the transitional rotation between the initial posture and the final posture, two rotation vectors perpendicular to the transitional rotation axis on the unit sphere can be obtained

Indicates that the two rotation vectors are perpendicular to the rotation axis

The included angle is the rotation angle θ. When one vector rotates to another vector, a circular arc is formed on the unit sphere. These two vectors and the circular arc represent the quaternion q _m of transitional rotation. Evenly distribute the points on the arc, then you can get a series of transition rotation four elements that are evenly distributed

After being superimposed with the initial pose, a series of uniform interpolated poses are obtained

The specific steps of quaternion attitude interpolation are given below.

Step 1. Obtain the rotation axis vector and rotation angle of the transitional rotation from the given initial posture and end posture, and the related equation is q _m =q _s ⁻¹ ·q _e .

Step 2. From the parametric equation of the space arc, a series of uniform points are taken on the rotation arc, and the space arc on the unit sphere can be expressed as the following equation.

为过渡转动的转轴矢量，θ为转角，弧所在平面的法向矢量也就是

初始姿态与结束姿态给定时，这些值是定值。t为空间圆弧参数，均匀选取参数t，可以得到圆弧上一系列均匀的点。in

is the rotation axis vector of the transitional rotation, θ is the rotation angle, and the normal vector of the plane where the arc is located is

When the initial attitude and end attitude are given, these values are fixed values. t is the parameter of the arc in space, if the parameter t is uniformly selected, a series of uniform points on the arc can be obtained.

从圆弧起点到终点间均匀选取各点，得到一系列向量

对过渡转动的四元数q_m均匀化，即Step 3. From the starting point vector of the arc to

Select points uniformly from the start point to the end point of the arc to get a series of vectors

Homogenize the quaternion q _m of transition rotation, namely

的格式转换成非正交的普通格式。A quaternion with the rotation axis orthogonal to the vector

format into a non-orthogonal common format.

Step 4. Obtain the final series of quaternion attitude interpolation values, namely:

Step 4. Build an extended Kalman filter for data fusion

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

Z _k+1 ＝θ _k+1 +v _k+1 (11)

At each instant, the inertial measurement unit and the odometry return complex observations of the same parameter (attitude angle), for which we use the Kalman gain to compute a reliance on the uncertainty of the two sensor observations value. The Kalman gain at time k+1 is:

是一个单位矩阵，是观测噪声的协方差矩阵，一般由厂家给出精度或者通过实验统计进行噪声评估，这里先假设in

is an identity matrix, which is the covariance matrix of the observation noise. Generally, the accuracy is given by the manufacturer or the noise is evaluated through experimental statistics. Here we first assume that

R _k+1 = σ _θ (13)

where σ _θ denotes the variance of the observation noise output by the IMU with respect to the pose.

so:

The posterior estimate of the state variable is:

The covariance matrix of the posterior estimates of the state variables is:

Description of drawings

Fig. 1 is a flowchart of a multi-sensor asynchronous data fusion processing method based on data interpolation.

Figure 2 is the kinematics model of the mobile robot turtlebot3.

Figure 3 is a structural diagram of the unmanned driving simulation platform.

Figure 4 is a trajectory diagram of the fusion algorithm without adopting the time synchronization strategy.

Figure 5 is a trajectory diagram of the fusion algorithm adopting different time synchronization strategies.

Figure 6 is a comparison chart of yaw angle changes in each control group.

detailed description

The method for fusion processing of multi-sensor asynchronous data based on data interpolation of the present invention will be further described in detail below with reference to illustrations.

The algorithmic model of the present invention is based on certain assumptions, and said assumptions include:

Assumption 1: The controlled vehicles are all intelligent vehicles, which have functions such as perception, calculation, control, and communication;

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

Hypothesis 3: Assume that the noise of each observation follows a Gaussian distribution;

The example of the present invention is to use the mobile robot to simulate the driving behavior of the vehicle indoors, and only rely on the vehicle sensor to obtain the vehicle pose information. By setting up the experimental control group and performing related programming calculations with Matlab software, the purpose of verifying the performance of the mobile robot positioning system is achieved.

Step 1. Construction of the experimental platform

In order to simulate the normal driving scene of the vehicle in the intelligent Internet of Vehicles, this example uses the Turtlebot3 car to build an unmanned driving simulation platform. Turtlebot3 is the third-generation product in the TurtleBot series. It is driven by the robot intelligent driver Dynamixel. It is a small, programmable, ROS-based cost-effective mobile robot. Its kinematic model is shown in Figure 2. The Turtlebot3 robot is equipped with an inertial measurement unit (IMU) and an odometer (Odom). Through the built-in system function package, the vehicle's speed, angular velocity, acceleration and other state information can be obtained in real time. The IMU operates at 100Hz and the Odom operates at 10Hz. According to the differential wheel kinematics model of the vehicle, this example selects the following secant model as the kinematics equation of the mobile robot:

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.

Step 2. Vehicle state estimation

The unmanned driving platform relies on the ROS system, and completes real-time monitoring of the system environment and driving behavior control by writing the corresponding function package. The platform structure is shown in Figure 3.

As mentioned above, this example selects the vehicle position and vehicle attitude as the vehicle state quantity, so the estimated vector X=[xyθ] ^T .

In order to obtain the noise information of each sensor, before the experiment started, the vehicle was placed statically in the field, and it was sampled at equal intervals for a long time, and the data was calculated and analyzed, and the results of each state quantity of the vehicle were shown in Table 1. .

Table 1 Noise parameters

When the odometer is used to obtain speed information when the vehicle is stationary, there is a fixed noise of about 5*10-4m/s in the odometer, so that the vehicle moves at a constant speed of 0.1m/s, and the speed observation can be measured The standard deviation of the error is about 3*10-3m/s. When estimating the vehicle position, the above deviation value is taken as the standard deviation of the noise into the state estimator for estimation.

Step 3. Positioning accuracy test

In order to verify the positioning accuracy based on the wheel odometer and IMU fusion positioning algorithm, eight 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), the smart car starts from the origin of the coordinate system and passes through these eight target points in turn, each target The movement trajectory between points is independently planned by the navigation algorithm, and the positioning accuracy of the target point can be used to estimate the positioning accuracy of the movement process. The trajectories of each positioning method are shown in Figure 4 and Figure 5. In Figure 4, the control group ① is the trajectory of the fusion positioning algorithm without time synchronization strategy; the odom trajectory is the trajectory of the wheel odometer. The black track is the desired track. In Figure 5, the control group ② is the trajectory of the fusion positioning algorithm adopting the time synchronization strategy with similar time stamps, and the control group ③ is the trajectory of the fusion positioning algorithm adopting the time synchronization strategy of data interpolation. The specific coordinate values of each point in different trajectories are shown in Table 2. The standard deviation is used to calculate the positioning accuracy of the three time synchronization strategies, and the accuracy values are shown in Table 3.

Table 2 Coordinate values of each point in different trajectories

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

Step 4. Yaw Angle Change Test

In order to verify the effectiveness of the present invention to attitude estimation, record the experimental data of three groups of control groups in step 3, and draw the yaw angle change contrast curve, the result is as shown in Figure 6, and the yaw angle angle values of each control group are as shown in the table three shown. It can be seen that in the initial stage of vehicle operation, that is, the process of moving towards point 1, since there is no change in attitude angle, the positioning effects of the three control groups are not much different. Afterwards, the attitude angle of the vehicle changed during the steering process. The control group ① continued to accumulate errors, and the yaw angle error at point 3 exceeded 10°. As the vehicle continued to drive, the yaw angle of the control group ① gradually lost However, the yaw angle data values of the control group ② and the control group ③ can reflect the actual change more accurately (the instantaneous jump in the figure is caused by a small change between -180° and 180°), so it can be shown that Effectiveness of quaternion pose interpolation based on spherical vectors for pose fusion.

Table 3 Yaw angle values of each point in different trajectories

Step 5. Experimental results

The invention proposes a multi-sensor data fusion method based on data interpolation to solve the problem of vehicle asynchronous sensor data fusion. We combine data interpolation and pose estimation, so that the data required by the multi-sensor fusion algorithm and the data in the buffer area are the data at the matching time. The effectiveness of the proposed data interpolation method is verified through an experimental platform, and the experimental results show that the positioning algorithm implemented according to the positioning framework proposed by the present invention has the advantage of high precision.

Claims (6)

1. A multi-sensor asynchronous data fusion processing method based on data interpolation is characterized in that: the system comprises a dead reckoning module, a data buffering module, a time index module and a data interpolation module; the dead reckoning module carries out pose estimation according to a motion dynamic model of the vehicle and inputs the pose estimation to the state estimator as prior estimation; the data buffer module is used for receiving the measurement information of each sensor of the vehicle at the current moment and storing the measurement information into a corresponding data buffer container according to the time sequence; 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 utilizes the front frame data and the rear frame data of the locking position to realize the data interpolation of the sensor, and the data after the interpolation is used as the observation information to be input into the state estimator, so as to realize the posterior estimation of the vehicle position.

2. A multi-sensor asynchronous data fusion processing method based on data interpolation by using the system of claim 1, characterized in that: storing IMU sensor data with a timestamp in a data buffer container, selecting an odometer sensor as a core sensor, taking the data timestamp of the odometer as reference time, and traversing observation data, namely IMU information, in the data buffer container to realize accurate matching of the sensor data in time; the data type of the estimated state of the dead reckoning module comprises position coordinates and a yaw angle; the data type of the dead reckoning module comprises vehicle running data and vehicle state information measured by a vehicle-mounted sensor, and an observation model of the vehicle is obtained as follows:

Z _k+1 ＝θ _k+1 +v _k+1 (1)

wherein Z is _k+1 Represents the systematic observation at time k +1, θ _k+1 Attitude vector information, v, representing the IMU _k+1 Representing the observed 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 filtering structure, and the dead reckoning process is realized as follows:

s1, establishing a corresponding state space equation and an observation equation according to a dynamic model of a vehicle and carrying out prior prediction value;

s2, performing time indexing according to the data time stamp of the measurement information;

s3, carrying out data interpolation according to adjacent frame data and processing system observed quantity Z _k+1 ；

And S4, according to the priori predicted value obtained in the S2 and the system view measurement obtained in the S3, an extended Kalman filter is built to realize the estimation of the position and the attitude of the vehicle.

4. The multi-sensor asynchronous data fusion processing method based on data interpolation according to claim 3, characterized in that: according to the dynamic model of each vehicle, estimating the state quantity of the vehicle by using extended Kalman filtering, wherein the equation is as follows:

wherein x is _k ＝[X _k ,Y _k ,θ _k ] ^T Indicating the pose of the vehicle at time k, X _k ,Y _k Indicating the position of the vehicle, theta _k Representing the attitude of the vehicle in the navigation coordinate system;

representing the pose estimation value at the k +1 moment predicted by the motion model; u. u _k+1 ＝[V _Odo,X ,V _Odo,Y ,w _Odo ] ^T Representing the control quantity provided by the wheel type odometer for predicting the steps; the symbol "^" represents an estimated value, and the symbol "-" represents a predicted value;

wherein, P _k And Q _k+1 Are respectively the attitude x _k And process noise w _k+1 The covariance matrix of (a);

and

respectively a Jacobian matrix of the motion model and a Jacobian matrix of the process noise;

wherein,

jacobian matrix representing an observation model, R _k+1 A covariance matrix representing observed noise;

R _k+1 ＝σ _θ (5)

wherein σ _θ Observed noise variance with respect to attitude representing the IMU output;

at the moment k, the equation (2) carries out state estimation through a kinematic model and a pose transformation matrix of the vehicle to obtain a priori predicted value at the moment k +1

Equation (3) is used to calculate the covariance matrix of the prior estimates for the state estimator at time k +1, and equation (4) is based on the error covariance matrix of the prior estimates

And observed quantity error covariance matrix R _k+1 To obtain the Kalman gain K at the moment of K +1 _k+1 (ii) a The observed noise variance R of the sensor assumed by equation (5) _k+1 (ii) a Equation (6) by observed quantity Z _k+1 And a priori predicted values

Obtaining the state estimation value of the vehicle at the k +1 moment

Equation (7) passes the Kalman gain K at time K +1 _k+1 Error covariance matrix with a priori predicted values

Obtaining an estimation error covariance matrix P at the moment of k +1 _k+1 。

5. The multi-sensor asynchronous data fusion processing method based on data interpolation according to claim 1, characterized in that: the data buffer module can independently store the sensor data in respective containers according to a time sequence, and the time index module can take the time stamp of the core sensor data as reference time and traverse in the containers storing other sensor data until finding a corresponding position; the rules for traversal are as follows:

the first condition is as follows: if the time stamp of the first data in the container is later than the reference time, it indicates that if data interpolation is performed here, the data at the previous moment of the reference time cannot be obtained, i.e. no slave insertion occurs, the traversal process exits, and the data time stamp at the next moment of the core sensor is selected as the reference time;

and (2) carrying out a second condition: if the constraint condition that the time stamp of the first data in the container is earlier than the reference time is met, the time stamp of the second data in the container must be ensured to be later than the reference time, otherwise, the first data is meaningless, the traversal is continued, and the first data is deleted;

and (3) carrying out a third condition: if the first condition and the second condition are met, but the difference value between the timestamp of the first data and the reference time is larger than the set threshold value, the data is proved to have the situation of data frame loss at the moment, the traversal process is quitted, and the data timestamp of the next moment of the core sensor is selected as the reference time;

and a fourth condition: if the conditions I, II and III are met, but the difference value between the timestamp of the second data and the reference time is larger than the set threshold value, the data is proved to have the situation of data frame loss at the moment, the traversal process is quitted, and the data timestamp of the next moment of the core sensor is selected as the reference time;

if the traversal process can simultaneously satisfy the four limiting conditions, the time index is considered to be completed, and the position is locked for data interpolation.

6. The multi-sensor asynchronous data fusion processing method based on data interpolation according to claim 1, characterized in that: the data interpolation module selects a corresponding interpolation method for interpolation according to the front frame data and the rear frame data of the locking position of the time index module so as to achieve accurate matching of the multi-sensor data in time; for the sensor data of the inertial measurement unit, a quaternion attitude interpolation method based on a spherical vector is selected for data interpolation, and an interpolation equation is as follows:

q _m ＝q _s ^-1 ·q _e (8)

wherein equation (8) is given by the initial pose q _s And ending attitude q _e To obtain a transitional rotation q _m (ii) a Uniformly selecting a series of points on a spatial arc on a unit spherical surface given by equation (9) according to a uniform selection parameter t; equation (10) by uniformly selecting points

For quaternion q of transitional rotation _m Homogenizing; equation (11) by initial attitude q _s And transitional rotation q _m To obtain a series of interpolation of quaternion poses.

Images