Visual inertial navigation method, device, equipment and computer readable storage medium

Abstract

The present invention discloses a visual inertial navigation method, device, equipment and computer-readable storage medium. The method includes: obtaining a state prediction value at the current moment by accumulating inertial navigation sensor data and estimating the state of the visual module at the previous moment; determining the tracked feature points and their observation values, determining the observation value estimation value of the feature points through the multi-camera state, and performing difference calculation between the observation value estimation value and the observation value to obtain a residual term; correcting the state prediction value according to the residual term to obtain a pose estimation at the current moment. A visual inertial navigation odometer solution integrating depth information is implemented, which improves the accuracy of the odometer, enhances the stability of the odometer in weak light or weak texture environments, and saves system overhead.

Description

Technical Field

[0001] The present invention relates to the field of inertial navigation technology, and in particular to a visual inertial navigation method, device, equipment and computer-readable storage medium. Background Art

[0002] In the prior art, VIO (Visual Inertial Odometry) refers to an algorithm that uses data from visual sensors (camera) and inertial sensors (IMU, Inertial measurement unit) to estimate the current position and posture of the system. This algorithm has important applications in autonomous mobile vision modules, unmanned vehicles, drone positioning and navigation, AR (Augmented Reality, augmented reality), etc. As an effective sensor for relative positioning of mobile vision modules, visual inertial odometry provides real-time position and posture information for the vision module. Compared with pure visual odometry, visual inertial odometry has the following advantages:

[0003] 1. Ability to obtain absolute scale estimation through IMU;

[0004] 2. It is more robust to the situation where the image is distorted due to the object moving too fast;

[0005] 3. In the event of a brief visual tracking failure, the system can continue to operate normally by relying on IMU data.

[0006] Although in theory the advantages and disadvantages of visual sensors and inertial sensors are complementary, the visual inertial odometer still has some shortcomings due to the low-cost inertial sensors. First, for low-cost civilian-grade inertial sensors, such as the inertial sensors used in mobile phones, or inertial sensors at the thousand-yuan level for vehicles, due to inaccurate precision, large noise and zero bias, drift may occur during operation, and the accumulated error will become larger and larger over time, resulting in inaccurate state estimation. Second, the stability of the visual inertial odometer is weak in low-light and low-texture environments. Summary of the invention

[0007] In order to solve the technical defects of poor accuracy and stability and low computational efficiency of visual inertial navigation odometers in the prior art, the present invention proposes a visual inertial navigation method, device, equipment and computer-readable storage medium. First, a visual inertial navigation method is proposed, which includes:

[0008] The current state prediction value is obtained by accumulating inertial navigation sensor data and estimating the state of the visual module at the previous moment;

[0009] Determine the tracked feature points and their observation values, determine the estimated observation values ​​of the feature points through the multi-camera state, and perform difference calculation between the estimated observation value and the observation value to obtain a residual term;

[0010] The state prediction value is corrected according to the residual term to obtain the pose estimate at the current moment.

[0011] The present invention also proposes a visual inertial navigation device, which includes:

[0012] The state prediction module is used to obtain the state prediction value at the current moment through the accumulation of inertial navigation sensor data and the state estimation of the visual module at the previous moment;

[0013] A residual construction module is used to determine the tracked feature points and their observation values, determine the estimated observation values ​​of the feature points through the multi-camera state, and perform difference calculation between the estimated observation value and the observation value to obtain a residual term;

[0014] The posture correction module is used to correct the state prediction value according to the residual term to obtain the posture estimate at the current moment.

[0015] The present invention also proposes a visual inertial navigation device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program implements the steps of the visual inertial navigation method described above when executed by the processor.

[0016] The present invention also proposes a computer-readable storage medium, on which a visual inertial navigation program is stored. When the visual inertial navigation program is executed by a processor, the steps of the visual inertial navigation method described above are implemented.

[0017] The beneficial effects of the present invention are that the state prediction value at the current moment is obtained by accumulating inertial navigation sensor data and estimating the state of the visual module at the previous moment; determining the tracked feature points and their observation values, determining the observation value estimation value of the feature points through the multi-camera state, and performing difference calculation between the observation value estimation value and the observation value to obtain the residual term; and correcting the state prediction value according to the residual term to obtain the pose estimation at the current moment. A visual inertial navigation odometer solution integrating depth information is realized, which improves the accuracy of the odometer, enhances the stability of the odometer in weak light or weak texture environments, and saves system overhead. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] The present invention will be further described below with reference to the accompanying drawings and embodiments, in which:

[0019] Figure 1is a first flow chart of the visual inertial navigation method provided by an embodiment of the present invention;

[0020] Figure 2 is a second flow chart of the visual inertial navigation method provided by an embodiment of the present invention;

[0021] Figure 3 is a third flow chart of the visual inertial navigation method provided by an embodiment of the present invention;

[0022] Figure 4 is a fourth flow chart of the visual inertial navigation method provided by an embodiment of the present invention;

[0023] Figure 5 is a fifth flow chart of the visual inertial navigation method provided by an embodiment of the present invention;

[0024] Figure 6 is a sixth flow chart of the visual inertial navigation method provided by an embodiment of the present invention;

[0025] Figure 7 is a seventh flow chart of the visual inertial navigation method provided by an embodiment of the present invention;

[0026] Figure 8 is an eighth flow chart of the visual inertial navigation method provided by an embodiment of the present invention;

[0027] Fig. 9 4 is a structural block diagram of a visual inertial navigation device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0028] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.

[0029] In the subsequent description, the suffixes such as "module", "component" or "unit" used to represent elements are only used to facilitate the description of the present invention, and have no specific meanings. Therefore, "module", "component" or "unit" can be used in a mixed manner.

[0030] Embodiment 1

[0031] Figure 1 is a first flow chart of a visual inertial navigation method provided by an embodiment of the present invention. This embodiment proposes a visual inertial navigation method, the method comprising:

[0032] S1, obtain the current state prediction value through the accumulation of inertial navigation sensor data and the state estimation of the visual module at the previous moment;

[0033] S2. Determine the tracked feature points and their observation values, determine the estimated observation values ​​of the feature points through the multi-camera state, and perform difference calculation between the estimated observation value and the observation value to obtain a residual term;

[0034] S3. Correct the state prediction value according to the residual term to obtain the pose estimate at the current moment.

[0035] In this embodiment, first, the position and posture of the visual module is predicted from the previous moment to the current state by accumulating IMU data. Among them, it should be noted that the visual inertial navigation method of this embodiment is applicable to devices or modules with autonomous positioning functions such as navigators and visual modules. The overall scheme of this embodiment is to use a filtering method to estimate the position and posture of the system at the current moment through the position and posture of the previous moment and the sensor data at the current moment. Specifically, in this embodiment, a visual module with a basic visual inertial navigation component is used as an example for explanation. The visual module is configured with corresponding visual components and inertial navigation components, wherein the required image information and depth information are obtained through the visual component, and the required inertial navigation sensor data is obtained through the inertial navigation component. In the previous steps of this embodiment, first, the position and posture of the visual module are initialized, and then, in the process of starting to execute the visual inertial navigation task, considering that the frequency of the inertial navigation component obtaining the inertial navigation sensor data is higher than the frequency of the visual component obtaining the image information, therefore, the position and posture of the visual module is predicted from the initial state to the current state by accumulating IMU data.

[0036] In this embodiment, the state of the visual module is first initialized, and the initialization includes initializing the world coordinate system, the state of the visual module, and the bias of the IMU accelerometer and the angular velocity meter. Then, the coordinate estimation value of the feature point in the world coordinate system is determined based on the RGB image or grayscale image obtained by the visual module in combination with the depth map, and the corresponding observation value estimation is calculated in combination with the coordinate estimation value. The residual term is constructed with the actual observation value, and the state of the visual module is corrected by using the residual term to obtain the pose estimation value of the visual module at the current moment. Among them, since the IMU frequency is higher than the visual module frequency, each time a frame of image is obtained, it is necessary to use the IMU data to accumulate the state of the previous moment to predict the state of the current moment. Subsequently, the previous frame image and the next frame image are subjected to feature extraction and matching to obtain feature points and their observation values ​​and corresponding depths that are relevant to the current moment. For feature points with invalid depth, their corresponding observation values ​​are regarded as 2D observations, otherwise they are regarded as 3D observations. And according to the observation value, the coordinate estimation value of the feature point in the world coordinate system is calculated respectively, and the world coordinate estimation value of the feature point is used and back-projected to the multi-camera state to calculate the corresponding observation value estimation value (including 2D observation estimation value and 3D observation estimation value), and the observation value estimation value and the actual observation value are differenced to obtain 2D residual term and 3D residual term. Then, the 2D residual term is first used to correct the pose prediction of the visual module to obtain the first pose estimation, and finally the 3D residual term is used to correct the first pose estimation value to obtain the pose estimation of the visual module at the current moment.

[0037] In this embodiment, after obtaining the pose estimation of the visual module at the current moment, useless information is deleted, specifically, key frames that are too old and key frames that have no associated feature points, and the corresponding information in the filter state is deleted. Further, the above-mentioned state update operation, residual acquisition operation and pose correction operation are repeatedly performed to obtain the pose information in the latest state in real time until the visual inertial navigation task is completed.

[0038] The beneficial effect of this embodiment is that the posture of the visual module is predicted from the initial state to the current state through the accumulation of IMU data; then, the coordinate estimation value of the feature point in the world coordinate system is determined by obtaining an RGB image or grayscale image in combination with a depth image according to the visual module, and the feature points are classified according to whether the depth value is valid and whether it is within the preset threshold range, and the 2D observation value estimation value and 3D observation value estimation value of the feature point are calculated according to the corresponding category, and the residual terms are constructed with the corresponding actual observation values ​​respectively; finally, the posture in the current state is corrected according to the residual terms, first using the 2D residual to correct, and then using the 3D residual to correct, to obtain the posture estimation in the latest state. A visual inertial navigation odometer solution that integrates depth information is realized, which improves the accuracy of the odometer, enhances the stability of the odometer in weak light or weak texture environments, and at the same time saves system overhead.

[0039] Embodiment 2

[0040] Figure 2 is a second flow chart of the visual inertial navigation method provided by an embodiment of the present invention. Based on the above embodiment, the state prediction value at the current moment is obtained by accumulating inertial navigation sensor data and estimating the state of the visual module at the previous moment, including:

[0041] S11. In the time period from the last moment to the current moment, the acceleration count value and the angular velocity count value in the inertial navigation sensor data are accumulated, and at the same time, the state prediction value is obtained in combination with the state estimation of the visual module at the last moment.

[0042] In this embodiment, when the required RGB image or grayscale image combined with the depth image is acquired according to the preset image acquisition frequency, since the frequency of the inertial navigation component acquiring the inertial navigation sensor data is higher than the frequency of the visual component acquiring the image information, the acceleration count value and the angular velocity count value in the inertial navigation sensor data are accumulated during the time period from the initial moment to the current moment, so as to obtain the state prediction value of the visual module in the current state.

[0043] Specifically, for example, first, determine the attitude of the vision module at a moment t. Then, record the time of delta t after the moment t as the moment t + 1. In order to obtain the state estimation at the moment t + 1, in this embodiment, the acceleration value and the angular velocity value in the inertial sensing data will be used for corresponding accumulation, so as to obtain the state prediction value at the above-mentioned moment t + 1.

[0044] Optionally, in this embodiment, before accumulating the acceleration value and the angular velocity value in the inertial sensing data, it is also necessary to determine whether the currently available IMU data can be initialized. Specifically:

[0045] First, obtain two IMU data segments of N (0.5 < N < 20) seconds. The first data segment is the data at a relatively newer moment, and the second data segment is the data at an older moment. Use the first IMU data segment to determine whether the variance of the inertial sensing data within a preset time period is greater than a preset threshold.

[0046] Then, if the variance of the above-mentioned inertial sensing data is greater than the preset threshold, proceed to the next initialization step. If the variance of the inertial sensing data is less than the preset threshold, it is determined that the initialization fails, and continue to obtain the inertial sensing data until the variance of the IMU values within the first data segment is greater than the threshold, indicating that the IMU is sufficiently excited. Among them, the first data segment is mainly used to judge that the module starts to move. It can be understood that the first data segment is relatively old data, and it is actually the data when the module is stationary before starting to move, and it will be used for initialization.

[0047] Optionally, in this embodiment, after determining that the currently available IMU data can be initialized, then perform the initialization operation. Specifically:

[0048] First, when the first IMU data segment is judged to be sufficiently excited, obtain the second IMU data segment for specific initialization.

[0049] Then, if the excitation is sufficient, use the second IMU data segment, that is, the old IMU data segment to initialize the IMU state. First, accumulate and average the IMU data segment, and regard the vector norm of the average value as an estimated value of the gravitational acceleration constant. Subsequently, construct the gravity vector (0, 0, -gravity_norm), and combine the acceleration average value to solve the rotation quaternion of the IMU and the world coordinate system. Since the IMU is in a relatively stationary state when this data segment is obtained, the zero bias of the IMU is set as the IMU noise, and至此 the initialization is successful.

[0050] It should be understood that in this embodiment, as described in the above example, if the calculated variance is greater than the above-mentioned set threshold, it means that the above-mentioned inertial navigation is effectively stimulated and can be initialized, and the bias of the accelerometer and the angular velocity meter is calculated using the second IMU data segment, wherein the bias represents the difference between the measurement data obtained by the current inertial navigation sensor component and the actual data.

[0051] The beneficial effects of this embodiment are that, on the one hand, by accumulating the acceleration count values ​​and angular velocity count values ​​in the inertial navigation sensor data in the time period from the previous moment to the current moment, an initialization judgment basis and an initialization execution plan are provided; on the other hand, combined with the state estimation of the visual module at the previous moment, the posture prediction of the visual module in the current state is obtained, which provides a data basis for subsequent valuation correction.

[0052] Embodiment 3

[0053] Figure 3 is the third flow chart of the visual inertial navigation method provided by an embodiment of the present invention. Based on the above embodiment, the tracked feature points and their observation values ​​are determined, the estimated observation values ​​of the feature points are determined through the multi-camera state, and the difference between the estimated observation value and the observation value is calculated to obtain the residual term, including:

[0054] S21. Perform feature extraction and matching based on the RGB image or grayscale image acquired by the visual module to obtain tracked feature points and their observation values.

[0055] In this embodiment, after initialization is completed, first, the corresponding RGB image or grayscale image is obtained through the visual module, and then, according to the current positioning requirements and the corresponding parameter configuration, the relevant feature extraction and matching operations are performed to obtain the tracked feature points and their observation values.

[0056] In this embodiment, by performing feature extraction and matching on the RGB image or grayscale image acquired by the visual module, the tracked feature points and their observation values ​​are obtained, thereby providing a data basis for subsequent coordinate transformation and difference calculation.

[0057] Embodiment 4

[0058] Figure 4 is the fourth flow chart of the visual inertial navigation method provided by an embodiment of the present invention. Based on the above embodiment, the tracked feature points and their observation values ​​are determined, the observation value estimation value of the feature points is determined through the multi-camera state, and the difference between the observation value estimation value and the observation value is calculated to obtain the residual term, including:

[0059] S22. Calculate the estimated coordinates of the feature points in the world coordinate system in combination with the observation values ​​of the feature points in the multi-camera state, and then project the estimated coordinates into the multi-camera state to obtain the estimated observation values ​​of the feature points.

[0060] In this embodiment, a certain point and C1, C2, C3 represent the camera states at three moments as an example for explanation. First, the actual observation values ​​m1, m2, m3 of the feature point are obtained according to the camera states C1, C2, C3 at the above three moments; then, the coordinate estimation value of the feature point in the world coordinate system is calculated using the camera states C1, C2, C3 at the above three moments and the three actual observation values ​​m1, m2, m3, and then, the coordinate estimation value is back-projected to the camera states C1, C2, C3 at the above three moments to obtain the three observation value estimation values ​​of the feature point for the camera states C1, C2, C3 at the above three moments. It can be understood that this embodiment is not limited to using the camera states at three moments for a certain feature point, but can use camera states of corresponding scales according to actual positioning requirements and corresponding parameter configurations.

[0061] The beneficial effect of this embodiment is that by combining the observation values ​​of the feature points in the multi-camera state, the coordinate estimation values ​​of the feature points in the world coordinate system are calculated, and then the coordinate estimation values ​​are projected into the multi-camera state to obtain the observation value estimation values ​​of the feature points, thereby providing a corresponding calculation scheme and data basis for the subsequent process.

[0062] Figure 5 is the fifth flow chart of the visual inertial navigation method provided by an embodiment of the present invention. Based on the above embodiment, in combination with the observation value of the feature point in the multi-camera state, the coordinate estimation value of the feature point in the world coordinate system is calculated, and then the coordinate estimation value is projected to the multi-camera state to obtain the observation value estimation value of the feature point, including:

[0063] S221, determining a depth value of the feature point in the corresponding depth map according to the observed value, and determining whether the depth value is within a preset depth threshold range;

[0064] S222: If the depth value is within a preset depth threshold range, calculating a coordinate estimate of the feature point in the world coordinate system or for a corresponding camera state by weighted least squares method, and back-projecting the calculated coordinates into the multi-camera state to obtain a second observation value estimate of the feature point in the multi-camera state;

[0065] S223. If the depth value is not within the depth threshold range, the coordinate estimate of the feature point in the world coordinate system or for the corresponding camera state is calculated by triangulation, and the calculated coordinates are back-projected into the multi-camera state to obtain a first observation value estimate of the feature point in the multi-camera state.

[0066] Specifically, in order to determine whether the current image frame contains valid depth information, in this embodiment, first, a preset depth threshold range is set according to the parameter information of the visual component and the corresponding computing requirements. Then, if the depth value of the feature point is not within the depth threshold range, it indicates that the depth value of the feature point is inaccurate. The two-dimensional observation of the feature point in the multi-camera state is used through a triangulation calculation scheme to obtain the coordinate estimate of the feature point in the world coordinate system, which is recorded as the first observation value estimate. If the depth value of the feature point is within the depth threshold range, it indicates that the depth value of the feature point is valid or accurate. Here, the two-dimensional observation and depth observation (i.e., three-dimensional observation) of the feature point in the multi-camera state are used through the weighted least squares calculation scheme to obtain the coordinate estimate of the feature point in the world coordinate system, which is recorded as the second observation value estimate.

[0067] In order to further explain the implementation of this embodiment, a specific example will be used below to illustrate how to calculate the coordinate estimation value in the world coordinate system through the feature points with depth information.

[0068] First, we construct a 3D feature point uncertainty model. Each frame of RGB image (red, green, and blue color mode image) corresponds to a frame of range image (depth map), where the depth value of the pixel point in the RGB image is the value of the corresponding coordinate in the range image. j , depending on its corresponding depth value d j is a random variable with mean The variance is Gaussian distribution. Also assume that the coordinates of the pixel point on the image (μ j , v j ) also follows a Gaussian distribution Its distribution is determined by the depth values ​​surrounding the pixel.

[0069] Specifically, let the observed depth value of a feature point in the camera coordinate system be Indicates the feature point f observed under camera pose i j The depth observation value of The model is modeled to conform to the mixed Gaussian distribution, and its distribution is determined by the depth values ​​of the pixels around the pixel, thereby obtaining an uncertain model of the depth observation value. According to the camera model, it can be deduced that the feature point is in the camera coordinate system. The distribution of values, corresponding to the expectation is

[0070] Finally, the feature point f is observed under the camera pose i j The observed value is Corresponding covariance ∑ i

[0071] For the same feature point, we have multiple sets of observations u = {u1 … u n} and the corresponding covariance ∑={∑1 ...∑ n}

[0072] The mapping relationship between the feature point coordinates in the world coordinate system and the coordinates in the camera coordinate system is:

[0073] The weighted least squares method can be used to obtain the estimated value of the coordinates of the feature point in the world coordinate system.

[0074] in, Represents the rotation matrix between the IMU coordinate system and the camera coordinate system at time i. Similarly, the left subscripts can be G world coordinate system, C camera coordinate system, and I IMU coordinate system respectively;

[0075] P generally refers to the coordinate value, such as cP I Represents the coordinates of the IMU in the camera coordinate system;

[0076] It is agreed that symbols with a ~ superscript are error values, and symbols with a ^ superscript are measured values ​​or estimated values.

[0077] Since the weighted least squares method is used for solution, the iterative process of algorithms such as LM and GN is omitted, thereby reducing the amount of calculation and improving the calculation efficiency.

[0078] So far, we have explained how to calculate the world coordinate system coordinates of feature points with depth information. However, in the actual task execution process, not all feature points have valid depth information. The solution of this embodiment is to set an effective threshold for the depth. If the depth is lower than the threshold and the pixels around the feature point have valid depth, then the feature point is regarded as a feature point with valid depth, classified as a 3D feature, and calculated using weighted least squares. The feature points other than these are classified as 2D features and are calculated accordingly using triangulation.

[0079] It should be noted that the above solution is based on the mainstream MSCKF (A multi-state constraint Kalman filter for vision-aided inertial navigation) and integrates depth sensor information. The state vector mathematical model is consistent with MSCKF.

[0080] Specifically, the state vector used in this embodiment is described below:

[0081] IMU state vector:

[0082] The parameters in the above state vector respectively represent the conversion quaternion from the world coordinate system to the IMU coordinate system, the coordinates of the IMU in the world coordinate system, the velocity of the IMU body in the world coordinate system, the bias of the angular accelerometer, the bias of the accelerometer, the conversion quaternion from the IMU coordinate system to the camera coordinate system, the coordinates of the camera in the IMU coordinate system, and the time difference between the IMU and the camera for acquiring data.

[0083] Filter state vector:

[0084] The first item is the current state of the IMU, and the following N items are the camera states of the sliding window size. It should be noted that the IMU state of this embodiment refers to the coordinates of the IMU in the world coordinate system and the conversion relationship between the IMU and the world coordinates.

[0085] The beneficial effect of this embodiment is that by determining a preset depth threshold range; if the depth value of the feature point is not within the depth threshold range, the coordinate estimate of the feature point in the world coordinate system is calculated by triangulation to obtain a first observation value estimate; if the depth value of the feature point is within the depth threshold range, the coordinate estimate of the feature point in the world coordinate system is calculated by the weighted least squares method to obtain a second observation value estimate. This embodiment increases the computational complexity of the visual inertial navigation task, saves system overhead, and improves processing efficiency.

[0086] Embodiment 6

[0087] Figure 6 is the sixth flow chart of the visual inertial navigation method provided by an embodiment of the present invention. Based on the above embodiment, the determination of the tracked feature points and their observation values, the determination of the estimated observation values ​​of the feature points through the multi-camera state, and the difference calculation of the estimated observation values ​​and the observed values ​​to obtain the residual term also include:

[0088] S23, performing difference calculation on the second observation value estimate and the observation value to obtain a three-dimensional residual term;

[0089] S24. Calculate the difference between the first observation value estimate and the observation value to obtain a two-dimensional residual term.

[0090] In this embodiment, on the one hand, the difference between the second observation value estimate and the observation value is calculated to obtain a three-dimensional residual term, and on the other hand, the difference between the first observation value estimate and the observation value is calculated to obtain a two-dimensional residual term.

[0091] Specifically, in this embodiment, the above-mentioned pose estimation is corrected by using the residual without depth features and the residual with depth features, respectively, so as to obtain the pose estimation in the latest state.

[0092] As described in the above example, after obtaining the above estimation, we can find the residual r between the estimated value and the actual observed value of the feature point observation. Since this residual is correlated with the feature point, we need to project the relevant part into the left null space to obtain Then update the state vector and covariance matrix using the following formula. Modify the 2D features and 3D features respectively; finally get the corresponding X t|t .

[0093] Specifically, the update formula is as follows:

[0094]

[0095]

[0096]

[0097]

[0098] The beneficial effect of this embodiment is that a three-dimensional residual term is obtained by performing a difference calculation on the second observation value estimate and the observation value, and a two-dimensional residual term is obtained by performing a difference calculation on the first observation value estimate and the observation value, thereby providing a data basis for two iterative corrections for subsequent pose estimation.

[0099] Embodiment 7

[0100] Figure 7 is the seventh flow chart of the visual inertial navigation method provided by an embodiment of the present invention. Based on the above embodiment, the state prediction value is corrected according to the residual term to obtain the pose estimation at the current moment, including:

[0101] S31, performing a first correction on the state prediction value by using the two-dimensional residual term;

[0102] S32. Perform a second correction on the pose estimate after the first correction through the three-dimensional residual term to obtain the pose estimate at the current moment.

[0103] Optionally, in this embodiment, if in the above embodiment, the depth values ​​of the feature points are all within the preset depth threshold range, the above-mentioned two-dimensional residual term is not generated, and the state prediction value is only corrected by the three-dimensional residual term to obtain the pose estimate at the current moment.

[0104] Optionally, in order to determine the depth value of the feature point, and further determine whether the depth value is within the above-mentioned preset depth threshold range, in this embodiment, a depth map is acquired by a depth camera;

[0105] Optionally, in this embodiment, the depth value of the feature point is obtained by a 3D laser ranging module, a stereoscopic photography module or other components with ranging function, so as to facilitate determination of whether the depth value is within the above-mentioned preset depth threshold range.

[0106] The beneficial effect of this embodiment is that the depth value of the feature point in the corresponding depth map is determined by the observation value, and whether the depth value is within the preset depth threshold range; if the depth value is within the preset depth threshold range, the coordinate estimation value of the feature point in the world coordinate system or for the corresponding camera state is calculated by weighted least squares method, and the calculated coordinates are back-projected into the multi-camera state to obtain the second observation value estimation value of the feature point in the multi-camera state; if the depth value is not within the depth threshold range, the coordinate estimation value of the feature point in the world coordinate system or for the corresponding camera state is calculated by triangulation method, and the calculated coordinates are back-projected into the multi-camera state to obtain the first observation value estimation value of the feature point in the multi-camera state. Thus, the pose estimation in the latest state is obtained, the calculation quality of the visual inertial navigation task is improved, the system overhead is saved, and the processing efficiency is improved.

[0107] Embodiment 8

[0108] Figure 8It is the eighth flow chart of the visual inertial navigation method provided by an embodiment of the present invention. Based on the above embodiments, in this embodiment, a complete execution flow of a visual inertial navigation task is given. Specifically, in this embodiment, first, the required data is obtained through the sensor, wherein the data includes two parts, one is the image data containing depth information, and the other is the inertial navigation sensor data; then, it is initialized by the inertial navigation sensor data within a preset time period. First, it is determined whether the variance of the IMU data within the time period is large enough. If it is large enough, it means that the initialization can continue. Otherwise, the data is continued to be acquired until the variance is large enough, that is, the IMU is effectively stimulated, and the values ​​of the accelerometer and angular velocity meter in the previous period are used to calculate the corresponding acceleration and angular velocity bias values ​​and the conversion relationship between the IMU coordinate system and the world coordinate system. The speed of the IMU in the world coordinate system is set to 0, and it is considered that the IMU is at the (0, 0, 0) coordinate of the world coordinate system at this time. At this point, it is considered that the system is initialized successfully. When the next frame of image is acquired, the prediction of the system state at the current moment is obtained by accumulating IMU data, and extracting feature points from the RGB image or grayscale image; until the second frame of image is acquired, the IMU data is accumulated to obtain the IMU state prediction at the moment of acquisition of the second frame of image. Feature points are extracted and matched from the RGB image or grayscale image to obtain feature points including the tracked feature points, and the validity is judged based on the depth information of the feature points to obtain the corresponding 2D features and 3D features. The 2D features are triangulated to obtain the corresponding world coordinate system coordinate value estimates, and the 3D features are weighted least squares to calculate the coordinate value estimates of the feature points in the world coordinate system. The coordinate estimates of the feature points in the world coordinate system are used to calculate the corresponding two-dimensional observation value estimates and three-dimensional observation value estimates, thereby obtaining two-dimensional residual terms and three-dimensional residual terms. The two-dimensional residual terms are used to make the first correction to the system state, and the three-dimensional residual terms are used to correct the system again to obtain the final system state estimate. At the same time, during the execution cycle of a visual inertial navigation task, the above-mentioned state update operation, residual acquisition operation and posture correction operation are repeatedly executed to obtain the posture information in the latest state in real time until the visual inertial navigation task is completed.

[0109] It can be understood that in this embodiment, the depth information is integrated into the visual inertial navigation scheme, and the depth information is selectively fully utilized to convert the feature point information that was originally only two-dimensionally observed (u, v coordinate values ​​of the image) into three-dimensional information (u, v, d) observation. On the one hand, the visual inertial navigation scheme can restore the scale more accurately. On the other hand, the weighted least squares method is used when calculating the coordinates of the feature points in the world coordinate system, which can effectively save the amount of calculation caused by iterative solutions similar to the LM algorithm. At the same time, because the weighted least squares method has higher accuracy and higher success rate in solving the feature point coordinates than the triangulation method, the correct operation of the visual inertial navigation task can still be maintained when the feature points are sparse or the ambient light is weak.

[0110] Embodiment 9

[0111] Fig. 9 4 is a structural block diagram of a visual inertial navigation device provided by an embodiment of the present invention.

[0112] The present invention also proposes a visual inertial navigation device, which includes:

[0113] The state prediction module 10 is used to obtain the state prediction value at the current moment by accumulating the inertial navigation sensor data and estimating the state of the visual module at the previous moment;

[0114] The residual construction module 20 is used to determine the tracked feature points and their observation values, determine the estimated observation values ​​of the feature points through the multi-camera state, and perform difference calculation between the estimated observation value and the observation value to obtain the residual term;

[0115] The posture correction module 30 is used to correct the state prediction value according to the residual term to obtain the posture estimation at the current moment.

[0116] It should be noted that the above-mentioned device embodiment and method embodiment belong to the same concept, and the specific implementation process is detailed in the method embodiment, and the technical features in the method embodiment are correspondingly applicable in the device embodiment, which will not be repeated here.

[0117] Embodiment 10

[0118] The present invention also proposes a visual inertial navigation device, characterized in that the visual inertial navigation device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, and when the computer program is executed by the processor, the steps of the visual inertial navigation method described above are implemented.

[0119] It should be noted that the above-mentioned device embodiment and method embodiment belong to the same concept, and the specific implementation process is detailed in the method embodiment, and the technical features in the method embodiment are correspondingly applicable in the device embodiment, which will not be repeated here.

[0120] Embodiment 11

[0121] The present invention also proposes a computer-readable storage medium, on which a visual inertial navigation program is stored. When the visual inertial navigation program is executed by a processor, the steps of the visual inertial navigation method described above are implemented.

[0122] It should be noted that the above-mentioned medium embodiment and method embodiment belong to the same concept, and the specific implementation process is detailed in the method embodiment, and the technical features in the method embodiment are correspondingly applicable in the medium embodiment, which will not be repeated here.

[0123] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0124] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0125] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, a magnetic disk, or an optical disk), and includes a number of instructions for enabling a terminal (which can be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0126] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the purpose of the present invention and the claims, which all fall within the protection of the present invention.

Claims

1. A visual inertial navigation method, characterized in that: The method comprises: The current state prediction value is obtained by accumulating inertial navigation sensor data and estimating the state of the visual module at the previous moment; Determine the tracked feature points and their observation values, determine the estimated observation values ​​of the feature points through the multi-camera state, and perform difference calculation between the estimated observation value and the observation value to obtain a residual term; Correcting the state prediction value according to the residual term to obtain a pose estimate at the current moment; The determining of the tracked feature points and their observation values, determining the estimated observation values ​​of the feature points through the multi-camera state, and performing difference calculation between the estimated observation value and the observation value to obtain a residual term, includes: Combined with the observation values ​​of the feature points in the multi-camera state, the coordinate estimation values ​​of the feature points in the world coordinate system are calculated, and then the coordinate estimation values ​​are projected into the multi-camera state to obtain the observation value estimation values ​​of the feature points.

2. The visual inertial navigation method according to claim 1, characterized in that: The state prediction value at the current moment is obtained by accumulating the inertial navigation sensor data and estimating the state of the visual module at the previous moment, including: In the time period from the last moment to the current moment, the acceleration count value and the angular velocity count value in the inertial navigation sensor data are accumulated, and at the same time, the state prediction value is obtained in combination with the state estimation of the visual module at the last moment.

3. The visual inertial navigation method according to claim 1, characterized in that: The determining of the tracked feature points and their observation values, determining the estimated observation values ​​of the feature points through the multi-camera state, and performing difference calculation between the estimated observation value and the observation value to obtain a residual term, includes: Feature extraction and matching are performed based on the RGB image or grayscale image acquired by the visual module to obtain the tracked feature points and their observation values.

4. The visual inertial navigation method according to claim 1, characterized in that: The step of combining the observation values ​​of the feature points in the multi-camera state to calculate the estimated coordinate values ​​of the feature points in the world coordinate system, and then projecting the estimated coordinate values ​​to the multi-camera state to obtain the estimated observation values ​​of the feature points includes: Determining a depth value of the feature point in the corresponding depth map according to the observed value, and determining whether the depth value is within a preset depth threshold range; If the depth value is within a preset depth threshold range, calculating a coordinate estimate of the feature point in the world coordinate system or for the corresponding camera state by weighted least squares method, and back-projecting the calculated coordinates into the multi-camera state to obtain a second observation value estimate of the multi-camera state for the feature point; If the depth value is not within the depth threshold range, the coordinate estimate of the feature point in the world coordinate system or for the corresponding camera state is calculated by triangulation, and the calculated coordinates are back-projected into the multi-camera state to obtain the first observation value estimate of the multi-camera state for the feature point.

5. The visual inertial navigation method according to claim 4, characterized in that: The step of determining the tracked feature points and their observation values, determining the estimated observation values ​​of the feature points through the multi-camera state, and performing difference calculation between the estimated observation values ​​and the observation values ​​to obtain a residual term, further includes: Performing difference calculation on the second observation value estimate and the observation value to obtain a three-dimensional residual term; And, the difference between the first observation value estimate and the observation value is calculated to obtain a two-dimensional residual term.

6. The visual inertial navigation method according to claim 5, characterized in that: The step of correcting the state prediction value according to the residual term to obtain a pose estimate at the current moment includes: Performing a first correction on the state prediction value by using the two-dimensional residual term; And, the pose estimate after the first correction is corrected for a second time through the three-dimensional residual term to obtain the pose estimate at the current moment.

7. A visual inertial navigation device, characterized in that: The device comprises: The state prediction module is used to obtain the state prediction value at the current moment through the accumulation of inertial navigation sensor data and the state estimation of the visual module at the previous moment; A residual construction module is used to determine the tracked feature points and their observation values, determine the estimated observation values ​​of the feature points through the multi-camera state, and perform difference calculation between the estimated observation value and the observation value to obtain a residual term; A posture correction module, used to correct the state prediction value according to the residual term to obtain a posture estimate at the current moment; The residual construction module is also used to combine the observation values ​​of the feature points in the multi-camera state, calculate the coordinate estimation value of the feature point in the world coordinate system, and then project the coordinate estimation value into the multi-camera state to obtain the observation value estimation value of the feature point.

8. A visual inertial navigation device, characterized in that: The visual inertial navigation device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the computer program is executed by the processor, the steps of the visual inertial navigation method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a visual inertial navigation program, and when the visual inertial navigation program is executed by a processor, the steps of the visual inertial navigation method according to any one of claims 1 to 6 are implemented.

Images