Visual and inertial information fused VIO zero-speed detection and correction method
By combining visual images and IMU data, the zero-speed state is detected using generalized likelihood ratio test method and constructing zero-speed observation for Kalman filter update, the pose estimation drift problem of VIO system in the zero-speed state is solved. Accurate detection of zero-speed state and accurate correction of posture estimation are achieved.
Patent Information
- Application Number
- CN202410933936.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-12
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-07-12
AI Technical Summary
The VIO system has a pose estimation drift problem in the zero-speed state. The existing solution cannot accurately distinguish the zero-speed and uniform-speed linear driving states, and it is difficult to determine the threshold, resulting in mis-detection and missed detection.
By combining visual image information and IMU data, zero-speed detection is performed using generalized likelihood ratio detection, fusing camera and IMU information source, and building zero-speed observation measurement for extended Kalman filter updates, avoiding the limitations of threshold judgment .
Accurate detection of zero-speed state and accurate correction of position estimation are achieved, missed detection of pure visual detection and confusing and misdetection of pure IMU detection, and improved the robustness and accuracy of detection.
Smart Images

Figure CN118960723A_ABST
Abstract
Description
Technical Field
[0001] The present invention provides a VIO zero-speed detection and correction method that integrates vision and inertial information, which mainly solves the problem of pose estimation drift of a visual inertial odometry (VIO) in a zero-speed state. The technology belongs to the field of robot / unmanned vehicle positioning and navigation. Background Art
[0002] VIO, or visual inertial odometry, is a position tracking technology that uses both a camera and an inertial measurement unit (IMU) to estimate position and direction by fusing visual and inertial information. In the fields of robotics and unmanned driving, VIO is widely used in tasks such as attitude estimation and autonomous navigation. However, due to the inevitable zero bias and noise in IMU measurement data, in the zero-speed state, on the one hand, the zero bias estimation of the IMU will be inaccurate in the absence of effective excitation (zero speed), which will cause the IMU to continue to diverge in pre-integration; on the other hand, the static feature points will invalidate the constraints generated by the visual reprojection error, and cannot correct the posture divergence caused by the IMU zero bias. Therefore, the VIO system will produce trajectory drift, resulting in a large deviation in the posture estimation.
[0003] In the existing solutions, in order to solve the above problems, the IMU data is usually collected over a period of time and its average value is calculated. If the average value is less than the specified threshold, it is considered to be in a zero-speed state, and the observation equation is constructed, and the extended Kalman filter is combined to update and correct the posture. However, this type of solution has the following two main problems:
[0004] 1. Inability to distinguish from uniform linear motion: IMU measurement data consists of three-axis acceleration and angular velocity. In a stationary state, the three-axis acceleration and angular velocity data should be zero. However, when the carrier is in uniform linear motion, theoretically the three-axis acceleration and angular velocity data are also zero. Therefore, the distinction between the two motion states is not large enough, and uniform linear motion is easily detected as a zero-speed condition, resulting in incorrect posture updates.
[0005] 2. The threshold is difficult to determine and cannot cope with complex environments: The threshold is generally determined by experiments to obtain empirical values. If it is too large, it is easy to miss detection and fail to constrain the zero-speed condition; if it is too small, it is easy to cause false detection and bring wrong constraint information. In addition, the error of IMU will change due to factors such as temperature, humidity, and vibration. The pre-determined threshold may not be suitable for the actual usage scenario.
[0006] Therefore, in order to solve the posture drift problem of the VIO system in the zero-speed state, the current solution has problems such as zero-speed detection misdetection, missed detection and determination of appropriate judgment thresholds, which need to be solved urgently. Summary of the invention
[0007] The limitations of the current VIO system zero-speed detection and correction scheme are as follows: 1. It cannot be accurately distinguished from the uniform straight-line driving state. 2. The judgment threshold is difficult to determine and cannot cope with complex environments. The present invention combines visual image information and IMU data as a criterion to detect the zero-speed state, thereby avoiding confusion between the zero-speed state and the uniform straight-line state. In addition, compared to the traditional threshold-based zero-speed detection method, the present invention integrates the accelerometer and gyroscope measurement data and uses the generalized likelihood ratio test (GLRT) method for zero-speed detection. Compared with the method based on a single inertial sensor measurement data threshold, the GLRT method has better detection performance and ensures the simplicity of calculation.
[0008] The specific technical solution is:
[0009] A VIO zero-speed detection and correction method that integrates visual and inertial information, integrating the camera and IMU information sources to jointly detect the zero-speed state;
[0010] For the camera part, in the zero-speed state, the pixel displacement of the matching points in adjacent frames is used to determine whether it is currently in the zero-speed state;
[0011] For the IMU part, the generalized likelihood ratio test is applied by combining the acceleration and angular velocity information of the three axes. After the value is brought into the calculation formula, it is determined whether it is currently in a zero-speed state based on whether it is less than the likelihood ratio test threshold.
[0012] When both the camera and IMU information sources determine that the carrier is currently in a zero-speed state, a zero-speed correction is applied: that is, the carrier is considered to be in a zero-speed state, a zero-speed observation is constructed, and applied to the extended Kalman filter for posture update.
[0013] Among them, for the camera part, the camera zero speed detection based on the pixel displacement of the feature point is as follows:
[0014] The matching points between two adjacent frames have a certain degree of displacement. Let O be the optical center of the camera, the projection pixel point of the three-dimensional point P in the previous frame image is P1, and the coordinates in the image plane are (x, y). After the camera is displaced, the projection point of P in the next frame image moves to the position of P2 (x+u, y+v), and the displacement is (u, v). During the movement, the same feature point will be displaced on the imaging plane as the camera is displaced.
[0015] The method for the visual sensor to detect the zero-speed state is as follows: a threshold is set, and when it is determined that the pixel displacement of enough matching points in adjacent image frames is less than the threshold, the vehicle is considered to be in the zero-speed state;
[0016] 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 the zero-speed state. An empirical method is set: delete the first 10 pairs of feature points sorted from large to small according to the pixel displacement of the matching points, and select the pair with the largest pixel difference among the remaining matching feature points for comparison with the threshold.
[0017] Among them, for the IMU part, the IMU zero-speed detection based on the generalized likelihood ratio test is as follows:
[0018] The detection of zero-speed state is abstracted as a binary hypothesis testing problem. The detection method can be used in 0 With Η 1 Choose between two hypotheses, where Η 0 With Η 1 is defined as follows:
[0019] Η 0 :Vehicle non-zero speed state
[0020] Η 1 :The vehicle is at zero speed
[0021] Based on the input of a series of IMU measurement data, it is calculated in the following formula:
[0022]
[0023] In the above formula, N represents the length of the IMU data set; σ a With σ ω represent the Gaussian white noise standard deviation of the accelerometer and gyroscope respectively; and is the accelerometer and gyroscope measurement data at the corresponding moment; is the mean value of the IMU data of this group; when When accepting the hypothesis Η 1 , it is considered that the vehicle is in zero speed state at this time; the judgment threshold γ MG It needs to be designed reasonably according to the characteristics of the inertial sensor measurement data and the analysis results of the actual vehicle test data.
[0024] Furthermore, the zero-speed state of the extended Kalman filter is updated by:
[0025] When both the IMU and the camera determine that the current vehicle is in a zero-speed state, it is considered that the vehicle is in a zero-speed state at this time, and it is regarded as a pseudo-observation, and the zero-speed correction is applied to update the state quantity estimation; in the zero-speed state, the speed of the unmanned vehicle is zero, and the posture at adjacent moments is unchanged. Three error quantities, namely zero-speed error, position consistency error, and posture consistency error, are constructed as observation quantities for updating the extended Kalman filter EKF;
[0026] The estimated speed at zero speed is as follows:
[0027]
[0028] z zupt_v The zero-speed correction variable of the speed part, with a superscript, indicates the observed value, and without a superscript, indicates the true value, and the same is true as follows; Represents the velocity of the VIO carrier coordinate system relative to the world coordinate system;
[0029] The zero-speed error r used for EKF update zupt_v for:
[0030]
[0031] in express The differential increment of
[0032] The actual position change and estimated position change at zero speed are as follows:
[0033]
[0034] z zupt_p Represents the zero speed correction variable of the speed part; is the actual position at the latest moment in the sliding window. Under zero speed, it should be the same as the actual position at the second latest moment. Stay consistent; and They represent the position differential increments at the latest moment and the second latest moment in the sliding window, respectively, and are used for the position consistency error r updated by the EKF. zupt_p for:
[0035]
[0036] The actual attitude change and estimated attitude change at zero speed are as follows:
[0037]
[0038] where z zupt_q Represents the zero speed correction variable of the attitude part, is the actual posture at the latest moment in the sliding window. At zero speed, it should be the same as the actual position at the second latest moment. Stay consistent; Represents the multiplication between quaternions; attitude consistency error r used for EKF update zupt_q for:
[0039]
[0040] and They represent the attitude differential increments at the latest moment and the second latest moment in the sliding window respectively.
[0041] The zero-speed state detection is carried out jointly by the camera and IMU to detect the zero-speed state of the unmanned vehicle; under the zero-speed constraint, three error quantities, namely zero-speed error, position consistency error, and attitude consistency error, are constructed and used as observation quantities for updating the EKF to correct the positioning error of the entire visual inertial odometer.
[0042] The present invention considers the displacement size of image feature points, combines the IMU zero-speed detection method based on generalized likelihood ratio, and fuses the information sources of vision and IMU from the feature level to determine the zero-speed state, thereby avoiding missed detection due to a large number of dynamic objects in pure visual zero-speed detection and false detection caused by confusion with uniform speed driving conditions in pure IMU zero-speed detection, and can detect the zero-speed state more accurately and robustly; then, by constructing three error quantities of zero-speed error, position consistency error, and attitude consistency error as observation quantities for the state update of the extended Kalman filter, the drift problem of VIO's posture estimation in the zero-speed state is effectively corrected, and more accurate posture estimation can be provided in combination with the current VIO algorithm. BRIEF DESCRIPTION OF THE DRAWINGS
[0043] Figure 1 Flowchart of the VIO zero-speed detection and correction solution that integrates vision and inertial information;
[0044] Figure 2 It is a schematic diagram of the change of matching points of two adjacent frames during the motion process of the present invention. DETAILED DESCRIPTION
[0045] The specific technical solution of the present invention is described in conjunction with the accompanying drawings.
[0046] like Figure 1 As shown in the figure, a VIO zero-speed detection and correction method based on the fusion of visual and inertial information is used to detect the zero-speed state by fusing the two information sources of camera and IMU.
[0047] For the camera, in the zero-speed state, the images of adjacent frames of the camera are roughly consistent, so the pixel displacement of the matching point is used to determine whether it is currently in the zero-speed state;
[0048] For the IMU part, the generalized likelihood ratio test is applied by combining the acceleration and angular velocity information of the three axes. After the calculation formula is entered, it is determined whether the current state is zero speed based on whether the value is less than the likelihood ratio test threshold.
[0049] When the information sources of both the camera and the IMU determine that the carrier is currently in a zero-speed state, the zero-speed correction is applied: that is, the carrier is considered to be in a zero-speed state, a zero-speed observation is constructed, and applied to the extended Kalman filter for posture update. The technical details will be explained in detail below.
[0050] (1) Camera zero-speed detection based on feature point pixel displacement
[0051] By analyzing the characteristics of continuous image frames during motion, it can be found that the matching points between two adjacent frames will have a certain degree of displacement. Figure 2 As shown in the figure, O is the optical center of the camera, the projection pixel of the 3D point P in the previous frame is P1, and the coordinates in the image plane are (x, y). After the camera is displaced, the projection point of P in the next frame moves to the position of P2 (x+u, y+v), and the displacement is (u, v). During the movement, the same feature point will be displaced on the imaging plane as the camera is displaced.
[0052] When the zero-speed state occurs, without considering dynamic objects and changes in illumination, the image frames at this time have very significant characteristics: there is almost no change between adjacent frames, that is, the pixel displacement of the matching points of two adjacent image frames is very small, and the modulus of the displacement is close to 0. Therefore, the method for the visual sensor to detect the zero-speed state is as follows: set a threshold, and when it is judged that the pixel displacement of enough matching points in adjacent image frames is less than this threshold, it is determined that the vehicle is in a zero-speed state. However, the appearance of dynamic objects will cause too many moving pixels between two adjacent frames, which will cause trouble for the detection of the zero-speed state. In order to solve this problem, an empirical method is set: delete the first 10 pairs of feature points sorted from large to small according to the pixel displacement of the matching points, and select the pair with the largest pixel difference among the remaining matching feature points to compare with the threshold. In this way, the influence of dynamic objects on the visual sensor's detection of the zero-speed state is solved, and the robustness of zero-speed detection is improved.
[0053] (2) IMU zero-speed detection based on generalized likelihood ratio test
[0054] Regarding the use of IMU for zero-speed state detection, traditional methods include the acceleration variance method, acceleration amplitude method, and angular velocity energy method based on single inertial sensor measurement data, as well as the generalized likelihood ratio test (GLRT) method that comprehensively considers accelerometer and gyroscope measurement data. Compared with using a single inertial sensor measurement data, the GLRT method has better detection effect and a simpler and more unified calculation method. Therefore, the IMU data uses the GLRT method described below to detect the zero-speed state. Mathematically, the detection of the zero-speed state can be abstracted as a binary hypothesis testing problem. The detection method can be used in Η 0 With Η1 Choose between two hypotheses, where Η 0 With Η 1 is defined as follows:
[0055] Η 0 :Vehicle non-zero speed state
[0056] Η 1 :The vehicle is at zero speed
[0057] Then, based on the input of a series of IMU measurement data, it is calculated in the following formula:
[0058]
[0059] In the above formula, N represents the length of the IMU data set; σ a With σ ω represent the Gaussian white noise standard deviation of the accelerometer and gyroscope respectively; and is the accelerometer and gyroscope measurement data at the corresponding moment; is the mean value of the IMU data of this group. When accepting the hypothesis Η 1 , it is considered that the vehicle is in zero speed state. Judgment threshold γ MG It needs to be designed reasonably according to the characteristics of the inertial sensor measurement data and the analysis results of the actual vehicle test data.
[0060] (3) Zero-speed state update based on extended Kalman filter
[0061] When both the IMU and the camera determine that the current vehicle is in a zero-speed state, it is considered that the vehicle is in a zero-speed state at this time, and it is used as a pseudo observation (Pseudo Observation), and the zero-speed correction is applied to update the state quantity estimation. In the zero-speed state, the speed of the unmanned vehicle is zero, and the posture at adjacent moments remains unchanged. Therefore, three error quantities, namely zero-speed error, position consistency error, and posture consistency error, are constructed as observation quantities for updating the Extended Kalman Filter (EKF).
[0062] The actual speed and estimated speed at zero speed are as follows:
[0063]
[0064] z zupt_v The zero-speed correction variable of the speed part, with a superscript, indicates the observed value, and without a superscript, indicates the true value, and the same is true as follows; Represents the velocity of the VIO carrier coordinate system relative to the world coordinate system;
[0065] The zero-speed error r used for EKF update zupt_v for:
[0066]
[0067] in express The differential increment of
[0068] The actual position change and estimated position change at zero speed are as follows:
[0069]
[0070] z zupt_p Represents the zero speed correction variable of the speed part; is the actual position at the latest moment in the sliding window. Under zero speed, it should be the same as the actual position at the second latest moment. Stay consistent; and They represent the position differential increments at the latest moment and the second latest moment in the sliding window, respectively, and are used for the position consistency error r updated by the EKF. zupt_p for:
[0071]
[0072] The actual attitude change and estimated attitude change at zero speed are as follows:
[0073]
[0074] where z zupt_q Represents the zero speed correction variable of the attitude part, is the actual posture at the latest moment in the sliding window. At zero speed, it should be the same as the actual position at the second latest moment. Stay consistent; Represents the multiplication between quaternions; attitude consistency error r used for EKF update zupt_q for:
[0075]
[0076] and They represent the attitude differential increments at the latest moment and the second latest moment in the sliding window respectively.
[0077] By combining the above-mentioned camera with IMU to detect the zero-speed state, the zero-speed state of the unmanned vehicle can be detected more accurately and robustly. Therefore, under the zero-speed constraint, three error quantities, namely zero-speed error, position consistency error, and attitude consistency error, are constructed as observation quantities for updating the EKF and correcting the positioning error of the entire visual inertial odometer.
Claims
1. A VIO zero-speed detection and correction method based on the fusion of visual and inertial information, characterized in that: The two information sources of the fusion camera and IMU are used to jointly detect the zero-speed state; For the camera part, in the zero-speed state, the pixel displacement of the matching points in adjacent frames is used to determine whether it is currently in the zero-speed state; For the IMU part, the generalized likelihood ratio test is applied by combining the acceleration and angular velocity information of the three axes. After the value is brought into the calculation formula, it is determined whether it is currently in a zero-speed state based on whether it is less than the likelihood ratio test threshold. When both the camera and IMU information sources determine that the carrier is currently in a zero-speed state, a zero-speed correction is applied: that is, the carrier is considered to be in a zero-speed state, a zero-speed observation is constructed, and applied to the extended Kalman filter for posture update.
2. The VIO zero-speed detection and correction method of vision and inertial information fusion according to claim 1 is characterized in that: For the camera part, the camera zero speed detection based on the pixel displacement of feature points is as follows: The matching points between two adjacent frames have a certain degree of displacement. Let O be the optical center of the camera, the projection pixel point of the three-dimensional point P in the previous frame image is P1, and the coordinates in the image plane are (x, y). After the camera is displaced, the projection point of P in the next frame image moves to the position of P2 (x+u, y+v), and the displacement is (u, v). During the movement, the same feature point will be displaced on the imaging plane as the camera is displaced. The method for the visual sensor to detect the zero-speed state is as follows: a threshold is set, and when it is determined that the pixel displacement of enough matching points in adjacent image frames is less than the threshold, the vehicle is considered to be in the 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 the zero-speed state. An empirical method is set: delete the first 10 pairs of feature points sorted from large to small according to the pixel displacement of the matching points, and select the pair with the largest pixel difference among the remaining matching feature points for comparison with the threshold.
3. The VIO zero-speed detection and correction method of vision and inertial information fusion according to claim 1 is characterized in that: For the IMU part, IMU zero-speed detection based on generalized likelihood ratio test, the specific method is: The detection of the zero-speed state is abstracted as a binary hypothesis testing problem. The detection method can choose between the two hypotheses H0 and H1, where H0 and H1 are defined as follows: Η0: Vehicle non-zero speed state H1: The vehicle is at zero speed Based on the input of a series of IMU measurement data, it is calculated in the following formula: In the above formula, N represents the length of the IMU data set; σ a With σ ω represent the Gaussian white noise standard deviation of the accelerometer and gyroscope respectively; and is the accelerometer and gyroscope measurement data at the corresponding moment; is the mean value of the IMU data of this group; when When , we accept the hypothesis H1 and think that the vehicle is in zero speed state; the judgment threshold γ MG It needs to be designed reasonably according to the characteristics of the inertial sensor measurement data and the analysis results of the actual vehicle test data.
4. The VIO zero-speed detection and correction method of vision and inertial information fusion according to claim 1 is characterized in that: The zero-speed state of the extended Kalman filter is updated in the following specific method: When both the IMU and the camera determine that the current vehicle is in a zero-speed state, it is considered that the vehicle is in a zero-speed state at this time, and it is regarded as a pseudo-observation, and the zero-speed correction is applied to update the state quantity estimation; in the zero-speed state, the speed of the unmanned vehicle is zero, and the posture at adjacent moments is unchanged. Three error quantities, namely zero-speed error, position consistency error, and posture consistency error, are constructed as observation quantities for updating the extended Kalman filter EKF; The estimated speed at zero speed is as follows: z zupt_v The zero-speed correction variable of the speed part, with a superscript, indicates the observed value, and without a superscript, indicates the true value, and the same is true as follows; Represents the velocity of the VIO carrier coordinate system relative to the world coordinate system; The zero-speed error r used for EKF update zupt_v for: in express The differential increment of The actual position change and estimated position change at zero speed are as follows: z zupt_p Represents the zero speed correction variable of the speed part; is the actual position at the latest moment in the sliding window. Under zero speed, it should be the same as the actual position at the second latest moment. Stay consistent; and They represent the position differential increments at the latest moment and the second latest moment in the sliding window, respectively, and are used for the position consistency error r updated by the EKF. zupt_p for: The actual attitude change and estimated attitude change at zero speed are as follows: where z zupt_q Indicates the zero speed correction variable of the attitude part, is the actual posture at the latest moment in the sliding window. At zero speed, it should be the same as the actual position at the second latest moment. Stay consistent; Represents the multiplication between quaternions; attitude consistency error r used for EKF update zupt_q for: and They represent the attitude differential increments at the latest moment and the second latest moment in the sliding window respectively. The zero-speed state detection is carried out jointly by the camera and IMU to detect the zero-speed state of the unmanned vehicle; under the zero-speed constraint, three error quantities, namely zero-speed error, position consistency error, and attitude consistency error, are constructed and used as observation quantities for updating the EKF to correct the positioning error of the entire visual inertial odometer.
Citation Information
Patent Citations
Inertial navigation error correction method based on vehicle zero speed detection
CN110702104A
Initialization method and device for VIO (Visual Inertial Odometer) system
CN112798010A
Low-speed unmanned vehicle positioning method based on fusion of camera and consumer-level IMU
CN113503872A
Vision-assisted gait feature constraint pedestrian navigation method
CN115790585A
System to use digital cameras and other sensors in navigation
US9031782B1
Cited By
Self-adaptive zero-speed detection algorithm based on interval search under variable-speed motion condition
CN119197542A
Adaptive zero-speed detection method based on interval search under variable speed motion conditions
CN119197542B
Aircraft control method and device, aircraft and computer readable storage medium
CN119414823A
Low-cost vehicle-mounted navigation positioning method based on assistance of vision and double speedometers in GNSS denial environment
CN119437218A
