A visual and inertial information fusion VIO zero speed detection and correction method

Abstract

This invention provides a VIO zero-velocity detection and correction method that integrates visual and inertial information. It combines information from both a camera and an IMU (Inertial Measurement Unit) to detect zero-velocity states. For the camera, in a zero-velocity state, the pixel displacement of matching points in adjacent frames determines whether the current state is zero. For the IMU, it combines triaxial acceleration and angular velocity information, applies a generalized likelihood ratio test, and determines whether the current state is zero based on whether the calculated value is less than the likelihood ratio test threshold. When both the camera and IMU information sources indicate that the vehicle is currently in a zero-velocity state, zero-velocity correction is applied: the vehicle is considered to be in a zero-velocity state, a zero-velocity observation is constructed, and applied to an extended Kalman filter for pose update. This invention can more accurately and robustly detect zero-velocity states and effectively corrects the pose estimation drift problem of VIO in a zero-velocity state.

Description

Technical Field

[0001] This invention provides a method for zero-speed detection and correction of visual inertial odometry (VIO) by fusing visual and inertial information. It mainly solves the pose estimation drift problem of visual inertial odometry (VIO) in zero-speed state. This technology belongs to the field of robot / unmanned vehicle positioning and navigation. Background Technology

[0002] VIO, or Visual Inertial Odometry, is a position tracking technology that utilizes both a camera and an Inertial Measurement Unit (IMU) to estimate position and orientation by fusing visual and inertial information. In robotics, autonomous driving, and other fields, VIO is widely used for tasks such as attitude estimation and autonomous navigation. However, because IMU measurement data inevitably contains bias and noise, at zero speed, on the one hand, the IMU's bias estimation becomes inaccurate without effective excitation (at zero speed), causing the IMU to continuously diverge during pre-integration; on the other hand, stationary feature points cause the constraints generated by visual reprojection errors to fail, failing to correct the pose divergence caused by the IMU's bias. Therefore, the VIO system experiences trajectory drift, leading to significant deviations in pose estimation.

[0003] In existing solutions, to address the aforementioned issues, the common approach is to collect IMU data over a period of time, calculate its average value, and if the average value is less than a predetermined threshold, the system is considered to be in a zero-velocity state. An observation equation is then constructed, and an extended Kalman filter is used for pose updates and corrections. However, this approach suffers from the following two main problems:

[0004] 1. Inability to distinguish from uniform linear motion: IMU measurement data consists of acceleration and angular velocity along three axes. In a stationary state, the acceleration and angular velocity data of all three axes should be zero. However, when the vehicle is in uniform linear motion, theoretically, the acceleration and angular velocity data of all three axes should also be zero. Therefore, the distinction between the two motion states is not large enough, and it is easy to detect uniform linear motion as a zero-speed condition, resulting in incorrect pose updates.

[0005] Second, the threshold is difficult to determine and cannot cope with complex environments: The determination of the judgment threshold is generally based on empirical values ​​obtained through experiments. Choosing too large a threshold can easily lead to missed detections and fail to constrain zero-speed conditions; choosing too small a threshold can easily lead to false detections and incorrect constraint information. In addition, the error of the IMU can change due to factors such as temperature, humidity, and vibration, and the predetermined threshold may not be suitable for actual use scenarios.

[0006] Therefore, current solutions to the pose drift problem of VIO systems in zero-speed states have issues such as false detection and missed detection in zero-speed detection, and the determination of appropriate judgment thresholds that urgently need to be addressed. Summary of the Invention

[0007] To address the limitations of current VIO system zero-speed detection and correction schemes: 1. They cannot accurately distinguish between zero-speed and uniform linear motion states. 2. Determining the judgment threshold is difficult and they cannot handle complex environments. This invention combines visual image information with IMU data as a criterion for detecting zero-speed states, avoiding confusion between zero-speed and uniform linear motion states. Furthermore, compared to traditional threshold-based zero-speed detection methods, this invention integrates accelerometer and gyroscope measurement data, using the generalized likelihood ratio test (GLRT) method for zero-speed detection. Compared to methods based on threshold values ​​from a single inertial sensor measurement data, the GLRT method offers better detection performance while maintaining computational simplicity.

[0008] The specific technical solution is as follows:

[0009] A method for zero-speed detection and correction of VIO based on the fusion of visual and inertial information, which integrates information from both the camera and IMU to detect the zero-speed state;

[0010] For the camera, in zero-speed mode, the pixel displacement of the matching points in adjacent frames is used to determine whether the current state is zero-speed.

[0011] For the IMU part, combining the acceleration and angular velocity information of the three axes, the generalized likelihood ratio test is applied. After substituting into the calculation formula, it is determined whether the current state is zero velocity based on whether the value is less than the likelihood ratio test threshold.

[0012] When both the camera and IMU sources indicate that the carrier is currently in a zero-velocity state, a zero-velocity correction is applied: that is, the carrier is considered to be in a zero-velocity state, a zero-velocity observation is constructed, and it is applied to the extended Kalman filter for pose update.

[0013] Specifically, for the camera component, the method for zero-velocity detection based on feature point pixel displacement is as follows:

[0014] The matching points between two adjacent frames are displaced to a certain extent. Let O be the optical center of the camera, and let the projection pixel of the 3D point P in the previous frame be P1, with coordinates (x, y) in the image plane. After the camera is displaced, the projection point of P in the next frame moves to the position P2 (x+u, y+v), with a displacement of (u, v). During the motion, the same feature points will be displaced on the imaging plane as the camera moves.

[0015] The method for visual sensors to detect zero-speed states is as follows: a threshold is set, and when the pixel displacement of a sufficient number of matching points in adjacent image frames is less than this threshold, the vehicle is considered to be in a zero-speed state.

[0016] However, the presence of dynamic objects can lead to an excessive number of moving pixels between adjacent frames, which can cause problems in detecting zero-speed states. An empirical method is to delete the top 10 pairs of feature points sorted by the pixel displacement of the matching points from largest to smallest, and then select the pair with the largest pixel difference among the remaining matching feature points for comparison with the threshold.

[0017] Specifically, for the IMU portion, the method for zero-rate IMU detection based on the generalized likelihood ratio test is as follows:

[0018] The detection of the zero-velocity state is abstracted into a binary hypothesis testing problem. The detection method can choose between two hypotheses, H0 and H1, which are defined as follows:

[0019] H0: Vehicle is not at zero speed

[0020] H1: The vehicle is at zero speed.

[0021] The following formula is calculated by substituting a series of input IMU measurement data:

[0022]

[0023] In the above formula, N represents the length of the IMU data set; σ a With σ ω These represent the standard deviations of Gaussian white noise for the accelerometer and gyroscope, respectively. and The data are accelerometer and gyroscope measurements at the corresponding time points; This is the mean of the IMU data set; when At this time, accept hypothesis H1, assuming the vehicle is at zero speed; determine the threshold γ. MG The design needs to be based on the characteristics of the inertial sensor measurement data and the analysis results of the actual vehicle test data.

[0024] Furthermore, the zero-rate state update of the extended Kalman filter is specifically implemented as follows:

[0025] Once both the IMU and the camera determine that the vehicle is in a zero-speed state, it is considered to be in a zero-speed state and treated as a false observation. Zero-speed correction is then applied to update the state quantity estimates. In the zero-speed state, the vehicle's speed is zero and its pose remains unchanged in adjacent time intervals. Three error quantities are constructed: zero-speed error, position consistency error, and attitude consistency error, which are used as observations for updating the Extended Kalman Filter (EKF).

[0026] The estimated speed at zero speed is shown below:

[0027]

[0028] z zupt_v The zero-velocity correction variable represents the velocity component. With a superscript, it indicates the observed value; without a superscript, it indicates the true value. The same applies below. This indicates the velocity of the VIO carrier coordinate system relative to the world coordinate system;

[0029] Zero-rate error quantity r used for EKF update zupt_v for:

[0030]

[0031] in express The differential increment;

[0032] The actual position change and the estimated position change at zero velocity are shown below:

[0033]

[0034] z zupt_p The zero-velocity correction variable represents the velocity component; where It is the actual position at the latest moment in the sliding window, and should be compared with the actual position at the second new moment when the speed is zero. Maintain consistency; and These represent the differential increments of the positions at the latest moment and the second new moment in the sliding window, respectively, used for the position consistency error r in EKF updates. zupt_p for:

[0035]

[0036] The actual attitude change and the estimated attitude change at zero speed are shown below:

[0037]

[0038] Where z zupt_q The zero-velocity correction variable represents the attitude component. It represents the actual attitude at the latest moment in the sliding window, which should be compared with the actual position at the second new moment in the zero-velocity state. Maintain consistency; Represents the multiplication between quaternions; the pose consistency error r used for EKF update zupt_q for:

[0039]

[0040] and These represent the attitude differential increments at the latest moment and the second new moment in the sliding window, respectively.

[0041] Zero-speed state detection is performed by combining a camera and an IMU to detect the zero-speed state of the autonomous vehicle. Under the zero-speed constraint, three error quantities are constructed: zero-speed error, position consistency error, and attitude consistency error, which are used as observations for updating the EKF and correcting the positioning error of the entire visual inertial odometry.

[0042] This invention considers the displacement of image feature points and combines an IMU zero-speed detection method based on generalized likelihood ratio. It integrates information from both visual and IMU sources at the feature level to determine the zero-speed state, avoiding the missed detections caused by a large number of dynamic objects in pure visual zero-speed detection and the false detections caused by confusion with constant speed driving conditions in pure IMU zero-speed detection. This allows for more accurate and robust detection of the zero-speed state. Furthermore, by constructing three error quantities—zero-speed error, position consistency error, and attitude consistency error—as observations for the state update of the extended Kalman filter, it effectively corrects the pose estimation drift problem of VIO in the zero-speed state and can provide more accurate pose estimation when combined with current VIO algorithms. Attached Figure Description

[0043] Figure 1 Flowchart of VIO zero-speed detection and correction scheme based on the fusion of visual and inertial information;

[0044] Figure 2 This is a schematic diagram illustrating the changes in matching points between two adjacent frames during the motion process of this invention. Detailed Implementation

[0045] The specific technical solution of the present invention will be described in conjunction with the accompanying drawings.

[0046] like Figure 1 As shown, a VIO zero-speed detection and correction method that integrates visual and inertial information is presented, which combines information from both the camera and IMU to detect the zero-speed state.

[0047] Regarding the camera, in the zero-speed state, the images of adjacent frames of the camera are roughly the same. Therefore, the current state of zero speed is determined by judging the pixel displacement of the matching point.

[0048] For the IMU part, combining the acceleration and angular velocity information of the three axes, the generalized likelihood ratio test is applied. After substituting into the calculation formula, the current state of zero velocity is determined based on whether the value is less than the likelihood ratio test threshold.

[0049] Zero-velocity correction is applied only when both the camera and IMU sources determine that the carrier is currently in a zero-velocity state. This means assuming the carrier is in a zero-velocity state, constructing a zero-velocity observation, and applying it to the extended Kalman filter for pose updating. The technical details will be elaborated below.

[0050] (1) Camera zero-velocity detection based on feature point pixel displacement

[0051] Analyzing the characteristics of consecutive image frames during motion reveals that the matching points between adjacent frames will have a certain degree of displacement. Specifically, for example... Figure 2 As shown, O is the optical center of the camera. The projection pixel of the 3D point P in the previous frame image is P1, with coordinates (x, y) in the image plane. After the camera moves, the projection point of P in the next frame image moves to the position P2(x+u, y+v), with a displacement of (u, v). During the motion, the same feature points will be displaced on the imaging plane as the camera moves.

[0052] When a zero-speed state occurs, without considering dynamic objects and changes in lighting, the image frames exhibit a very significant characteristic: there is almost no change between adjacent frames, meaning the pixel displacement of matching points between adjacent image frames is very small, with the magnitude of the displacement approaching zero. Therefore, the method for visual sensors to detect zero-speed states is as follows: a threshold is set, and when a sufficient number of matching points in adjacent image frames have pixel displacements less than this threshold, the vehicle is considered to be in a zero-speed state. However, the presence of dynamic objects can lead to an excessive number of moving pixels between adjacent frames, complicating zero-speed state detection. To address this issue, an empirical method is adopted: the top 10 pairs of feature points, sorted by matching point pixel displacement from largest to smallest, are deleted, and the pair with the largest pixel difference among the remaining matching feature points is compared with the threshold. This resolves the impact of dynamic objects on the visual sensor's detection of zero-speed states, improving the robustness of zero-speed detection.

[0053] (2) Zero-rate detection of IMU based on generalized likelihood ratio test

[0054] Regarding zero-velocity state detection using IMUs, traditional methods include the acceleration variance method, acceleration amplitude method, and angular velocity energy method based on single inertial sensor measurement data. There is also the General Likelihood Ratio Test (GLRT), which comprehensively considers both accelerometer and gyroscope measurement data. Compared to using single inertial sensor measurement data, the GLRT method offers better detection results and a simpler, more unified calculation method. Therefore, the GLRT method described below is used for zero-velocity state detection using IMU data. Mathematically, zero-velocity state detection can be abstracted as a binary hypothesis testing problem, where the detection method can choose between two hypotheses, H0 and H1, defined as follows:

[0055] H0: Vehicle is not at zero speed

[0056] H1: The vehicle is at zero speed.

[0057] Then, based on a series of input IMU measurement data, the following formula is used for calculation:

[0058]

[0059] In the above formula, N represents the length of the IMU data set; σ a With σ ω These represent the standard deviations of Gaussian white noise for the accelerometer and gyroscope, respectively. and The data are accelerometer and gyroscope measurements at the corresponding time points; This is the mean of the IMU data set. When... At this point, hypothesis H1 is accepted, and the vehicle is considered to be at zero speed. The decision threshold γ... MG The design needs to be based on the characteristics of the inertial sensor measurement data and the analysis results of the actual vehicle test data.

[0060] (3) Zero-rate state update based on extended Kalman filter

[0061] Once both the IMU and the camera determine that the vehicle is currently in a zero-speed state, it is considered to be in a zero-speed state and treated as a pseudo-observation. Zero-speed correction is then applied to update the state quantity estimates. In the zero-speed state, the autonomous vehicle's speed is zero, and its pose remains unchanged between adjacent time points. Therefore, three error quantities—zero-speed error, position consistency error, and attitude consistency error—are constructed to serve as the observations for updating the Extended Kalman Filter (EKF).

[0062] The actual speed and estimated speed at zero speed are shown below:

[0063]

[0064] z zupt_v The zero-velocity correction variable represents the velocity component. With a superscript, it indicates the observed value; without a superscript, it indicates the true value. The same applies below. This indicates the velocity of the VIO carrier coordinate system relative to the world coordinate system;

[0065] Zero-rate error quantity r used for EKF update zupt_v for:

[0066]

[0067] in express The differential increment;

[0068] The actual position change and the estimated position change at zero velocity are shown below:

[0069]

[0070] z zupt_p The zero-velocity correction variable represents the velocity component; where It is the actual position at the latest moment in the sliding window, and should be compared with the actual position at the second new moment when the speed is zero. Maintain consistency; and These represent the differential increments of the positions at the latest moment and the second new moment in the sliding window, respectively, used for the position consistency error r in EKF updates. zupt_p for:

[0071]

[0072] The actual attitude change and the estimated attitude change at zero speed are shown below:

[0073]

[0074] Where z zupt_q The zero-velocity correction variable represents the attitude component. It represents the actual attitude at the latest moment in the sliding window, which should be compared with the actual position at the second new moment in the zero-velocity state. Maintain consistency; Represents the multiplication between quaternions; the pose consistency error r used for EKF update zupt_q for:

[0075]

[0076] and These represent the attitude differential increments at the latest moment and the second new moment in the sliding window, respectively.

[0077] By combining the aforementioned camera and IMU for zero-speed state detection, the zero-speed state of the autonomous vehicle can be detected more accurately and robustly. Thus, under the zero-speed constraint, three error quantities—zero-speed error, position consistency error, and attitude consistency error—are constructed as observations for updating the EKF (Electronic Kinematics Function) and correcting the overall positioning error of the visual inertial odometry system.

Claims

1. A method for VIO zero-speed detection and correction based on the fusion of visual and inertial information, characterized in that, The system combines information from both the camera and the IMU to detect zero-speed states. For the camera component, in a zero-speed state, the current state is determined by the pixel displacement of matching points in adjacent frames; the specific method is as follows: The matching points between two adjacent frames have a certain degree of displacement. Let O be the camera optical center, and let P1 be the projection pixel of the 3D point P in the previous frame image, with coordinates in the image plane as follows: After the camera shifts, the projection point of P in the next frame moves to P2. The position, the displacement is During the movement, the same feature points will be displaced on the imaging plane as the camera moves. The method for visual sensors to detect zero-speed states is as follows: a threshold is set, and when the pixel displacement of a sufficient number of matching points in adjacent image frames is less than this threshold, the vehicle is considered to be in a zero-speed state. However, the appearance of dynamic objects will result in too many moving pixels between two adjacent frames, which will cause trouble for the detection of zero speed state. An empirical method is set: delete the top 10 pairs of feature points sorted by the pixel displacement of the matching points from largest to smallest, and select the pair with the largest pixel difference among the remaining matching feature points to compare with the threshold. For the IMU part, combining the acceleration and angular velocity information of the three axes, the generalized likelihood ratio test is applied. After substituting into the calculation formula, the current state of zero velocity is determined based on whether the value is less than the likelihood ratio test threshold. When both the camera and IMU sources indicate that the carrier is currently in a zero-velocity state, a zero-velocity correction is applied: that is, the carrier is considered to be in a zero-velocity state, a zero-velocity observation is constructed, and the observation is applied to the extended Kalman filter for pose update.

2. The VIO zero-speed detection and correction method based on visual and inertial information fusion according to claim 1, characterized in that, For the IMU portion, a zero-rate detection method based on the generalized likelihood ratio test is employed, specifically as follows: The detection of the zero-velocity state is abstracted into a binary hypothesis testing problem, and the detection method can... and Choosing between two hypotheses, where and The definition is as follows: Vehicle not at zero speed The vehicle is at zero speed. The following formula is calculated by substituting a series of input IMU measurement data: ； In the above formula, N represents the length of measurement data for this series of IMUs; and These represent the standard deviations of Gaussian white noise for the accelerometer and gyroscope, respectively. and The data are accelerometer and gyroscope measurements at the corresponding time points; This is the average of the measurement data from this series of IMUs; when At that time, accept the hypothesis The threshold is set at which the vehicle is considered to be at zero speed. The design needs to be based on the characteristics of the inertial sensor measurement data and the analysis results of the actual vehicle test data.

3. The VIO zero-speed detection and correction method based on visual and inertial information fusion according to claim 1, characterized in that, The zero-rate state update of the extended Kalman filter is specifically performed as follows: Once both the IMU and the camera determine that the vehicle is in a zero-speed state, it is considered to be in a zero-speed state and treated as a false observation. Zero-speed correction is then applied to update the state quantity estimates. In the zero-speed state, the vehicle's speed is zero and its pose remains unchanged in adjacent time intervals. Three error quantities are constructed: zero-speed error, position consistency error, and attitude consistency error, which are used as observations for updating the Extended Kalman Filter (EKF). The estimated speed at zero speed is shown below: ； The zero-velocity correction variable represents the velocity component. With a superscript, it indicates the observed value; without a superscript, it indicates the true value. The same applies below. This indicates the velocity of the VIO carrier coordinate system relative to the world coordinate system; Zero-speed error quantity used for EKF update for: ； in express The differential increment; The actual position change and the estimated position change at zero velocity are shown below: ； The zero-velocity correction variable represents the velocity component; where It is the actual position at the latest moment in the sliding window, and should be compared with the actual position at the second new moment when the speed is zero. Maintain consistency; and These represent the differential increments of the positions at the latest moment and the second new moment in the sliding window, respectively, used for the position consistency error in EKF updates. for: ； The actual attitude change and the estimated attitude change at zero speed are shown below: ； in The zero-velocity correction variable represents the attitude component. It represents the actual attitude at the latest moment in the sliding window, which should be compared with the actual position at the second new moment in the zero-velocity state. Maintain consistency; Represents multiplication between quaternions; pose consistency error used for EKF updates. for: ； and These represent the attitude differential increments at the latest moment and the second new moment in the sliding window, respectively; Zero-speed state detection is performed by combining a camera and an IMU to detect the zero-speed state of the autonomous vehicle. Under the zero-speed constraint, three error quantities are constructed: zero-speed error, position consistency error, and attitude consistency error. These are used as observations for updating the EKF and correcting the positioning error of the entire visual inertial odometry system.

Images