Visual inertial odometer positioning method based on NCC dynamic adjustment covariance

Abstract

The invention discloses a VIO positioning method for dynamically adjusting covariance by using normalized cross-correlation (NCC), and the method comprises the steps: constructing a new observation model, introducing a tracking error of a feature point on a pixel value, taking the NCC as an index for quantifying the matching quality between the feature points, and reflecting the tracking error of the feature point by calculating the NCC matched with the feature points. Therefore, the covariance matrix of the observation noise item is dynamically adjusted to adapt to the change of the matching quality of the feature points, and the method can obtain a more accurate and robust matching result in a complex environment in which the matching quality difference of the feature points is significant.

Description

Technical Field

[0001] The application of visual-inertial odometry (VIO) in drone and robot navigation. Based on the Multi-State Constraint Kalman Filter (MSCKF), this paper proposes a visual-inertial odometry positioning method with dynamic covariance adjustment based on NCC. Background Art

[0002] With the increasing demand for positioning services, indoor positioning technology has become a research hotspot. In indoor environments, due to signal obstruction, the Global Navigation Satellite System (GNSS) and its related combined positioning systems cannot perform effective positioning. In this environment, Simultaneous Localization And Mapping (SLAM) is a commonly used positioning method. According to the different sensors used, it can be divided into Visual SLAM (VSLAM) and LiDAR SLAM (Light Detection And Ranging SLAM). Compared with LiDAR, visual sensors have the advantages of small size, low price, and rich perception information. In recent years, VSLAM technology with cameras as the main sensor has received widespread attention.

[0003] However, VSLAM is easily affected by ambient light and visual features, and is prone to tracking loss in situations such as fast motion, image blur, and weak texture areas, which has limitations. In contrast, the Inertial Measurement Unit (IMU) can provide high-frequency acceleration and angular velocity, can provide absolute scale for the camera, and assist in the extraction and matching of visual feature points. After the visual information is lost, the Inertial Navigation System (INS) can still work with high precision in a short time. Therefore, the complementarity of the two is conducive to fusion, which can improve the accuracy and robustness of the system. Especially in environments where GNSS signals are missing, the visual-inertial odometry (VIO) that integrates vision and IMU has become an important development direction of VSLAM technology. At present, VIO algorithms are mainly divided into two categories: filtering-based and optimization-based. Since the optimization method uses more observation data in each iteration cycle, the latter is generally considered to be more accurate than the former. However, the iterative solution of nonlinear equations requires a lot of computing resources and has high requirements on the computing power of the hardware platform. Although the extended Kalman filter (ESKF) based VSLAM has a high computational cost, it has a high computational cost. The method based on EKF (Extended Kernel Filter) has poor estimation accuracy, but has high computational efficiency and the unique advantage of being able to output accurate estimation uncertainty, which is an advantage that optimization-based methods do not have. Although existing VIO methods are developing towards higher accuracy and less computational effort, they pay insufficient attention to the poor quality of feature point matching caused by scenes such as texture changes, illumination changes, and motion blur. Analysis shows that both filtering methods and optimization methods usually assume that the noise term covariance matrix of the observation model is constant. However, the assumption of a constant covariance matrix fails to reflect this difference, resulting in a decrease in the system's performance when processing low-quality matches. This assumption ignores the difference in feature point matching quality between different frames, thereby reducing the system's robustness when the feature matching quality is poor. Summary of the Invention

[0004] To address the above issues, Solodar et al. proposed a method using deep learning networks to predict and adjust the noise of the observation model, thereby improving the performance of the system in different environments. However, this method requires a large amount of training data and computing resources; Huang et al. adopted a variational Bayesian method to model the measurement noise covariance matrix as an inverse Wishart distribution, and jointly estimated the noise covariance matrix and the state vector. However, this method is based on the fact that the noise covariance matrix has a constant dimension. In practice, the measurement dimension will also change due to changes in the environment. Therefore, it is not suitable for VIO systems where the measurement dimension changes over time. Yue et al. proposed a robust method based on fuzzy logic. Adaptive filter, using the trace of the innovative covariance matrix as the input of fuzzy inference, obtains the adjusted measurement noise covariance matrix, thereby improving the robustness of the VIO system. However, this method relies heavily on empirical parameters, which limits its accuracy and practical applicability. In practical applications, the noise covariance matrix is ​​also affected by the quality of feature point matching. For example, in the case of illumination changes, perspective changes and image blur, there are significant differences in the quality of feature point matching. In addition, the feature point matching process is often accompanied by a large number of outliers, which further affects the accuracy and robustness of the system. Based on the above problems, this paper proposes a VIO that dynamically adjusts the covariance based on normalized cross-correlation (NCC). This method dynamically adjusts the covariance matrix of the observation model by calculating the NCC of feature point matching. By introducing NCC into the VIO system, the covariance matrix can be dynamically adjusted according to the quality of feature point matching, thereby enhancing the robustness of the system under different matching quality conditions.

[0005] The visual inertial odometry positioning method based on NCC dynamic covariance adjustment includes the following steps:

[0006] Step 1: After receiving a frame of image, the system uses IMU attitude estimation to calculate the camera attitude estimation, thereby realizing state augmentation, the relationship between the camera attitude angle error and the quaternion, and the relationship between the camera attitude angle error and the rotation matrix;

[0007] Step 2: In the MSCKF algorithm, define the world coordinate system, camera coordinate system, and IMU coordinate system;

[0008] Step 3: Each time the camera measurement value of a key frame is obtained, a new camera pose state is added to the state vector and the state covariance matrix is ​​expanded and updated;

[0009] Step 4: After receiving the image, the system removes the mean of the grayscale values ​​of the two windows and calculates their covariance and standard deviation;

[0010] Step 5: Build an observation model. In the actual observation process, due to the existence of noise and error, tracking error is introduced. NCC is used as an indicator to quantify the matching quality between feature points.

[0011] Step 6: Use the observation residuals to update the state estimation, state correction, and error covariance correction to obtain the covariance matrix and gain matrix of the measurement information, so as to obtain more accurate and robust matching results in complex environments where the quality of feature point matching varies significantly.

[0012] In the MSCKF algorithm described in step 2, define the world coordinate system, camera coordinate system, and IMU coordinate system;

[0013] The specific steps are as follows:

[0014] Step 2-1, the state vector of the system at time is defined as follows:

[0015]

[0016] Where, represents the state vector of the IMU, and Represent the camera pose and position estimation corresponding to time i=1...n, respectively, and define the error state vector of the entire VIO system

[0017]

[0018] Where, Indicates the error state of the IMU, represents the position error of the camera, δθ represents the angular deviation of the error quaternion, and the propagation model of the state covariance matrix is ​​expressed in the following form:

[0019]

[0020] Where, represents the state covariance matrix of the IMU, represents the cross-covariance matrix of the camera and IMU poses,

[0021] The covariance matrix representing the camera pose;

[0022] Step 2-2: After receiving a frame of image, the system uses IMU attitude estimation to calculate the camera attitude estimation, thereby achieving state augmentation. The relationship between the camera attitude angle error and the quaternion, and the relationship between the camera attitude angle error and the rotation matrix are as follows:

[0023]

[0024] Where, Represents the rotation quaternion between the IMU and the camera frame; Indicates the position of the origin of the camera frame relative to the IMU; these two parameters can be calculated during offline calibration. Relative to The rotation matrix of .

[0025] As described in step 3, each time the camera measurement value of a key frame is obtained, a new camera pose state is added to the state vector and the state covariance matrix is ​​expanded, thereby updating it;

[0026] The specific steps are as follows:

[0027] Step 3-1, update the covariance matrix:

[0028]

[0029] Where, P k|k and P k|k * Respectively represent the covariance matrix before and after augmentation. The corresponding update of the Jacobian matrix can be obtained from the above formula:

[0030]

[0031] In the formula, the second row and third column is the identity matrix instead of the zero matrix, because during the state update process, the camera's position state has a direct impact on the update of the covariance matrix. In the state vector, the camera's position state directly affects the covariance matrix. Therefore, when the camera's position state changes, the covariance matrix also needs to be updated accordingly to reflect this change. If a zero matrix is ​​used, it means that the change in the camera's position state has no effect on the covariance matrix, which is inconsistent with the actual situation.

[0032] Step 3-2: After state augmentation, construct an observation model to update the error. Suppose that in a series of feature points, the i-th frame image observes the j-th feature point f j , represents the three-dimensional position of the feature point in the camera frame, Represents the two-dimensional position of the feature points in the image plane, and the corresponding relationship between them is:

[0033]

[0034] Where, represents the position of the jth feature in the i-th camera frame, Represents the coordinates of the jth feature point in the w coordinate system, Indicates the position coordinates of the i-th camera in the w system, represents the measurement white noise vector;

[0035] The reprojection error of the feature points is:

[0036]

[0037] Where, represents the estimated value of the two-dimensional position of the feature point;

[0038]

[0039] In the formula, the position of the feature points is estimated through a nonlinear optimization process using bundle adjustment and inverse depth parameterization. Minimize the reprojection error.

[0040] After receiving the image, the system described in step 4 removes the mean of the grayscale values ​​of the two windows and calculates their covariance and standard deviation;

[0041] The specific steps are as follows:

[0042] Step 4-1: Assume there are two feature points A and B, which are in the previous frame I1 and the current frame I2 respectively. Calculate the grayscale value P of the 3×3 pixel neighborhood centered on A and B. A and P B , then the grayscale mean of the two windows is:

[0043]

[0044] Step 4-2: Perform mean removal processing:

[0045]

[0046] In the formula, the mean removal formula is used to remove the mean of the pixel neighborhood grayscale values ​​of the two windows A and B;

[0047] Step 4-3, calculate its covariance and standard deviation:

[0048]

[0049] Step 4-4, calculate the NCC for each pair of feature points:

[0050]

[0051] In the construction of the observation model described in step 5, tracking errors are introduced due to the presence of noise and errors during the actual observation process, and NCC is used as an indicator to quantify the matching quality between feature points;

[0052] The specific steps are as follows:

[0053] Step 5-1: The residuals of the observation model need to satisfy the following form:

[0054] r=Hx+n (17)

[0055] There is a definite relationship between the pixel value of the visual feature point in the image and the position in the camera coordinate system. Let (u, v) be the pixel coordinates of the feature point on the image, (X, Y, Z) be the three-dimensional coordinates of the same feature point in the camera system, (f x ,f y ,c x ,c y ) is the internal parameter of the camera, and their relationship can be expressed as:

[0056]

[0057] In the actual tracking process, due to the existence of noise and error, the actual observed pixel coordinates of the feature points will be different from those in the ideal situation. Therefore, the tracking error ε is introduced, and then:

[0058]

[0059] Where ε is the uncertainty of feature point matching, which conforms to the Gaussian distribution with a mean of zero. 2×1 ~N(0,Σ x ), where ε uses NCC as an indicator to quantify the matching quality between feature points. The higher the NCC value, the more accurate the matching between feature points and the smaller the observation noise should be. Conversely, the lower the NCC value, the larger the observation noise should be. In the residual function of a single feature point and a single observation, the distribution of the noise term is:

[0060]

[0061] Then the distribution of all noise items at a single noise point is:

[0062]

[0063] The noise term distribution of all feature points is:

[0064]

[0065] Then Σ is the adaptive matrix R, which plays a role in calculating the Kalman gain.

[0066] The observation residuals described in step 6 are used to update the state estimation, state correction, and error covariance correction to obtain the covariance matrix and gain matrix of the measurement information, thereby obtaining more accurate and robust matching results in complex environments where the quality of feature point matching varies significantly;

[0067] The specific steps are as follows:

[0068] Step 6-1, in the state prediction step of the Kalman filter, the prior state and error covariance The prediction is made according to the following formula:

[0069]

[0070] Where, represents the estimated state vector at time t-1, represents the estimated state covariance matrix at time t-1, Q t-1 Represents the covariance matrix corresponding to the system noise, the observation vector Z t With the state vector X t The relationship between the observation model H t describe:

[0071] Z t =H t X t +v t (twenty four)

[0072] Where, v t Represents the observation noise, assuming it is zero-mean Gaussian white noise, and the Kalman gain K t The formula used to weigh predicted and observed values ​​is:

[0073]

[0074] Where R t Represents the covariance matrix corresponding to the measurement noise, using the observation residual The formulas for updating state estimation, state correction and error covariance correction are as follows:

[0075]

[0076] Let α=Σ=diag(Σ i ), then the covariance matrix and gain matrix of the measurement information are:

[0077]

[0078] Beneficial effects of the present invention:

[0079] 1. This paper proposes a method to use deep learning networks to predict and adjust the noise of observation models, thereby improving the performance of the system in different environments;

[0080] 2. This method creates a new observation model, introduces the tracking error of feature points on the pixel value, reflects the tracking error of feature points by calculating the NCC of feature point matching, and dynamically adjusts the covariance matrix of the noise term of the observation model to more accurately reflect the uncertainty of feature point matching;

[0081] 3. Experiments on open source datasets and data collected from underground parking lots in closed environments demonstrate that the proposed method significantly improves the accuracy and robustness compared to the original algorithm. In addition, the proposed method and the optimized VINS-MONO algorithm have both achieved significant improvements in accuracy and efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 This is a flow chart comparing the positioning methods using MSCKF algorithm and VINS-MONO algorithm in this paper;

[0083] Figure 2 This is a specific flow chart of step 2 of one embodiment of this invention;

[0084] Figure 3 This is a specific flow chart of step 3 of one embodiment of this invention;

[0085] Figure 4 This is a specific flow chart of state update using MSCKF algorithm in this paper;

[0086] Figure 5 This is the flowchart of the article summary;

[0087] Figure 6 This is a comparison chart of the time consumed by different algorithms in this article;

[0088] Figure 7 This is a comparison chart of the positioning trajectories of the three algorithms in this article;

[0089] Figure 8 It is the X-direction positioning error curve;

[0090] Figure 9 It is the Y direction positioning error curve;

[0091] Figure 10 This is the plane direction positioning error curve. DETAILED DESCRIPTION

[0092] An embodiment of the present invention will be further described below with reference to the accompanying drawings;

[0093] The specific steps are as follows:

[0094] Step 1: After receiving a frame of image, the system uses IMU attitude estimation to calculate the camera attitude estimation, thereby realizing state augmentation, the relationship between the camera attitude angle error and the quaternion, and the relationship between the camera attitude angle error and the rotation matrix;

[0095] Step 2: In the MSCKF algorithm, define the world coordinate system, camera coordinate system, and IMU coordinate system;

[0096] Step 2-1, the state vector of the system at time is defined as follows:

[0097]

[0098] Where, represents the state vector of the IMU, and Represent the camera pose and position estimation corresponding to time i=1...n, respectively, and define the error state vector of the entire VIO system

[0099]

[0100] Where, Indicates the error state of the IMU, represents the position error of the camera, δθ represents the angular deviation of the error quaternion, and the propagation model of the state covariance matrix is ​​expressed in the following form:

[0101]

[0102] Where, represents the state covariance matrix of the IMU, represents the cross-covariance matrix of the camera and IMU poses, The covariance matrix representing the camera pose;

[0103] Step 2-2: After receiving a frame of image, the system uses IMU attitude estimation to calculate the camera attitude estimation, thereby achieving state augmentation. The relationship between the camera attitude angle error and the quaternion, and the relationship between the camera attitude angle error and the rotation matrix are as follows:

[0104]

[0105] Where, Represents the rotation quaternion between the IMU and the camera frame; Indicates the position of the origin of the camera frame relative to the IMU; these two parameters can be calculated during offline calibration. Relative to The rotation matrix of .

[0106] As described in step 3, each time the camera measurement value of a key frame is obtained, a new camera pose state is added to the state vector and the state covariance matrix is ​​expanded, thereby updating it;

[0107] Step 3-1, update the covariance matrix:

[0108]

[0109] Where, P k|k and P k|k* Respectively represent the covariance matrix before and after augmentation. The corresponding update of the Jacobian matrix can be obtained from the above formula:

[0110]

[0111] In the formula, the second row and third column is the identity matrix instead of the zero matrix, because during the state update process, the camera's position state has a direct impact on the update of the covariance matrix. In the state vector, the camera's position state directly affects the covariance matrix. Therefore, when the camera's position state changes, the covariance matrix also needs to be updated accordingly to reflect this change. If a zero matrix is ​​used, it means that the change in the camera's position state has no effect on the covariance matrix, which is inconsistent with the actual situation.

[0112] Step 3-2: After state augmentation, construct an observation model to update the error. Suppose that in a series of feature points, the i-th frame image observes the j-th feature point f j , represents the three-dimensional position of the feature point in the camera frame, Represents the two-dimensional position of the feature points in the image plane, and the corresponding relationship between them is:

[0113]

[0114] Where, represents the position of the jth feature in the i-th camera frame, Represents the coordinates of the jth feature point in the w coordinate system, Indicates the position coordinates of the i-th camera in the w system, represents the measurement white noise vector;

[0115] The reprojection error of the feature points is:

[0116]

[0117] Where, represents the estimated value of the two-dimensional position of the feature point;

[0118]

[0119] In the formula, the position of the feature points is estimated through a nonlinear optimization process using bundle adjustment and inverse depth parameterization. Minimize the reprojection error.

[0120] After receiving the image, the system described in step 4 removes the mean of the grayscale values ​​of the two windows and calculates their covariance and standard deviation;

[0121] Step 4-1: Assume there are two feature points A and B, which are in the previous frame I1 and the current frame I2 respectively. Calculate the grayscale value P of the 3×3 pixel neighborhood centered on A and B. A and P B , then the grayscale mean of the two windows is:

[0122]

[0123] Step 4-2: Perform mean removal processing:

[0124]

[0125] In the formula, the mean removal formula is used to remove the mean of the pixel neighborhood grayscale values ​​of the two windows A and B;

[0126] Step 4-3, calculate its covariance and standard deviation:

[0127]

[0128] Step 4-4, calculate the NCC for each pair of feature points:

[0129]

[0130] In the construction of the observation model described in step 5, tracking errors are introduced due to the presence of noise and errors during the actual observation process, and NCC is used as an indicator to quantify the matching quality between feature points;

[0131] Step 5-1: The residuals of the observation model need to satisfy the following form:

[0132] r=Hx+n (17)

[0133] There is a definite relationship between the pixel value of the visual feature point in the image and the position in the camera coordinate system. Let (u, v) be the pixel coordinates of the feature point on the image, (X, Y, Z) be the three-dimensional coordinates of the same feature point in the camera system, (f x ,f y ,c x ,c y ) is the internal parameter of the camera, and their relationship can be expressed as:

[0134]

[0135] In the actual tracking process, due to the existence of noise and error, the actual observed pixel coordinates of the feature points will be different from those in the ideal situation. Therefore, the tracking error ε is introduced, and then:

[0136]

[0137] Where ε is the uncertainty of feature point matching, which conforms to the Gaussian distribution with a mean of zero. 2×1 ~N(0,Σ x ), where ε uses NCC as an indicator to quantify the matching quality between feature points. The higher the NCC value, the more accurate the matching between feature points and the smaller the observation noise should be. Conversely, the lower the NCC value, the larger the observation noise should be. In the residual function of a single feature point and a single observation, the distribution of the noise term is:

[0138]

[0139] Then the distribution of all noise items at a single noise point is:

[0140]

[0141] The noise term distribution of all feature points is:

[0142]

[0143] Then Σ is the adaptive matrix R, which plays a role in calculating the Kalman gain.

[0144] The observation residuals described in step 6 are used to update the state estimation, state correction, and error covariance correction to obtain the covariance matrix and gain matrix of the measurement information, thereby obtaining more accurate and robust matching results in complex environments where the quality of feature point matching varies significantly;

[0145] Step 6-1, in the state prediction step of the Kalman filter, the prior state and error covariance The prediction is made according to the following formula:

[0146]

[0147] Where, represents the estimated state vector at time t-1, represents the estimated state covariance matrix at time t-1, Q t-1 Represents the covariance matrix corresponding to the system noise, the observation vector Z t With the state vector X t The relationship between the observation model H t describe:

[0148] Z t =H t X t +v t (twenty four)

[0149] Where, v t Represents the observation noise, assuming it is zero-mean Gaussian white noise, and the Kalman gain Kt The formula used to weigh predicted and observed values ​​is:

[0150]

[0151] Where R t Represents the covariance matrix corresponding to the measurement noise, using the observation residual The formulas for updating state estimation, state correction and error covariance correction are as follows:

[0152]

[0153] Let α=Σ=diag(Σ i ), then the covariance matrix and gain matrix of the measurement information are:

[0154]

[0155] This paper proposes a method that uses a deep learning network to predict and adjust the noise of the observation model, thereby improving the system's performance in different environments. This method creates a new observation model, introduces the tracking error of feature points on the pixel value, reflects the tracking error of feature points by calculating the NCC of feature point matching, and dynamically adjusts the covariance matrix of the observation model noise term to more accurately reflect the uncertainty of feature point matching. Experiments on open source datasets and data collected in an underground parking lot in a closed environment demonstrate that this method significantly improves the accuracy and robustness of the original algorithm. It also significantly improves the accuracy and efficiency of this method compared to the optimized VINS-MONO algorithm.

[0156] like Figure 1 The figure shows a flow chart comparing the positioning methods using the MSCKF algorithm and the VINS-MONO algorithm, which describes in detail the process of obtaining the observation model and comparing the two algorithms.

[0157] like Figure 2 As shown in the figure, it is a process diagram for defining the world coordinate system, camera coordinate system and IMU coordinate system in the MSCKF algorithm and using the IMU attitude estimation to calculate the camera attitude estimation to achieve state augmentation;

[0158] like Figure 3 As shown in FIG, the process of removing the mean value of the grayscale value of the pixel neighborhood centered on the feature points A and B, calculating its covariance and standard deviation, and calculating its NCC for each pair of feature points;

[0159] like Figure 4 As shown, it is a VIO flow chart based on NCC dynamic adjustment of covariance;

[0160] like Figure 5 Shown is the summary flow chart of this article;

[0161] like Figure 6 As shown in the figure, it is a comparison of the time consumption of different algorithms. Compared with the original method, the proposed method only takes about 5ms more but the accuracy is greatly improved.

[0162] like Figure 7 As shown in the figure, the X-direction and Y-direction error curves show that the positioning errors of the MSCKF and VINS-MONO algorithms fluctuate greatly over time, especially during the periods of rapid turning or loss of visual feature points (around 160 seconds and 275 seconds), that is, at points B and D, the errors increase sharply. The maximum errors of the MSCKF algorithm in the X and Y directions exceed 0.6m.

[0163] like Figure 8 As shown, the X-direction positioning error curve;

[0164] like Figure 9 As shown, the Y direction positioning error curve;

[0165] like Figure 10 As shown, the plane direction positioning error curve;

[0166] The above is only the most basic specific implementation method of the present invention, but the scope of protection of the present invention is not limited to this. Any replacement that can be understood by anyone in this technical field within the technical scope disclosed by the present invention should be included in the scope of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A visual inertial odometer positioning method based on NCC dynamically adjusting covariance, characterized in that: The following steps are involved: Step 1: After receiving a frame of image, the system uses IMU attitude estimation to calculate the camera attitude estimation, thereby realizing state augmentation, the relationship between the camera attitude angle error and the quaternion, and the relationship between the camera attitude angle error and the rotation matrix; Step 2: In the MSCKF algorithm, define the world coordinate system, camera coordinate system, and IMU coordinate system; Step 3: Each time the camera measurement value of a key frame is obtained, a new camera posture state is added to the state vector and the state covariance matrix is ​​expanded, so it is updated; Step 4: After receiving the image, the system removes the mean of the grayscale values ​​of the two windows and calculates their covariance and standard deviation; Step 5: Construct an observation model. In the actual observation process, due to the existence of noise and error, tracking errors are introduced, and NCC is used as an indicator to quantify the matching quality between feature points. Step 6: Use the observation residuals to update the state estimation, state correction and error covariance correction to obtain the covariance matrix and gain matrix of the measurement information, so as to obtain more accurate and robust matching results in complex environments where the quality of feature point matching varies significantly.

2. The visual inertial odometer positioning method based on NCC dynamic covariance adjustment according to claim 1, characterized in that: In the MSCKF algorithm described in step 2, define the world coordinate system, camera coordinate system and IMU coordinate system; The specific steps are as follows: Step 2-1, the state vector of the system at time is defined as follows: In the formula, represents the state vector of the IMU, and They represent the camera pose and position estimation corresponding to time i=1...n, respectively, and define the error state vector of the entire VIO system In the formula, Indicates the error state of the IMU, represents the position error of the camera, δθ represents the angular deviation of the error quaternion, and the propagation model of the state covariance matrix is ​​expressed in the following form: In the formula, represents the state covariance matrix of the IMU, represents the cross-covariance matrix of the camera and IMU poses, The covariance matrix representing the camera pose; Step 2-2: After the system receives a frame of image, it uses IMU attitude estimation to calculate the camera's attitude estimation, thereby achieving state augmentation. The relationship between the camera attitude angle error and the quaternion, and the relationship between the camera attitude angle error and the rotation matrix are as follows: In the formula, Represents the rotation quaternion between the IMU and the camera frame; Indicates the position of the origin of the camera frame relative to the IMU; these two parameters can be calculated during offline calibration. Relative to The rotation matrix of .

3. The visual inertial odometer positioning method based on NCC dynamic covariance adjustment according to claim 1, characterized in that: As described in step 3, each time the camera measurement value of a key frame is obtained, a new camera posture state is added to the state vector and the state covariance matrix is ​​expanded, so it is updated; The specific steps are as follows: Step 3-1, update the covariance matrix: Where P k|k and P k|k * Respectively represent the covariance matrix before and after augmentation. From the above formula, the corresponding update of the Jacobian matrix is: In the formula, the third column of the second row is the unit matrix instead of the zero matrix, because in the state update process, the position state of the camera has a direct impact on the update of the covariance matrix. In the state vector, the position state of the camera directly affects the covariance matrix. Therefore, when the position state of the camera changes, the covariance matrix also needs to be updated accordingly to reflect this change. If the zero matrix is ​​used, it means that the change of the camera position state has no effect on the covariance matrix, which is inconsistent with the actual situation. Step 3-2: After state augmentation, construct an observation model to update the error. Suppose that in a series of feature points, the i-th frame image observes the j-th feature point f j , represents the three-dimensional position of the feature point in the camera frame, Represents the two-dimensional position of the feature points in the image plane, and the corresponding relationship between them is: In the formula, represents the position of the jth feature in the i-th camera frame, represents the coordinates of the jth feature point in the w coordinate system, represents the position coordinates of the i-th camera in the w system, represents the measured white noise vector; The reprojection error of the feature points is: In the formula, represents the estimated value of the two-dimensional position of the feature point; In the formula, the position of the feature points is estimated through a nonlinear optimization process using bundle adjustment and inverse depth parameterization. Minimize the reprojection error.

4. The visual inertial odometer positioning method based on NCC dynamic covariance adjustment according to claim 1, characterized in that: After receiving the image, the system described in step 4 removes the mean of the grayscale values ​​of the two windows and calculates their covariance and standard deviation; The specific steps are as follows: Step 4-1: Assume that there are two feature points A and B, which are in the previous frame I1 and the current frame I2 respectively. Calculate the gray value P of the 3×3 pixel neighborhood centered on A and B. A and P B , then the grayscale mean of the two windows is: Step 4-2: Perform mean removal processing: In the formula, the mean removal formula is used to remove the mean of the pixel neighborhood grayscale values ​​of the two windows A and B; Step 4-3, calculate its covariance and standard deviation: Step 4-4, calculate the NCC for each pair of feature points:

5. The visual inertial odometer positioning method based on NCC dynamic covariance adjustment according to claim 1, It is characterized in that In the construction of the observation model described in step 5, in the actual observation process, due to the existence of noise and error, tracking errors are introduced, and NCC is used as an indicator to quantify the matching quality between feature points; The specific steps are as follows: Step 5-1: The residuals of the observation model need to satisfy the following form: r=Hx+n (17) There is a definite relationship between the pixel value of the visual feature point in the image and its position in the camera coordinate system. Let (u, v) be the pixel coordinates of the feature point in the image, (X, Y, Z) be the three-dimensional coordinates of the same feature point in the camera system, and (f x ,f y ,c x ,c y ) is the internal parameter of the camera, and their relationship can be expressed as: In the actual tracking process, due to the existence of noise and error, the pixel coordinates of the feature points actually observed will be different from those in the ideal situation. Therefore, the tracking error ε is introduced, and then: Where ε is the uncertainty of feature point matching, which conforms to the Gaussian distribution with a mean of zero. 2×1 ~N(0,Σ x ), where ε uses NCC as an indicator to quantify the matching quality between feature points. The higher the NCC value, the more accurate the matching between feature points and the smaller the observation noise should be. On the contrary, the lower the NCC value, the larger the observation noise should be. In the residual function of a single feature point and a single observation, the distribution of the noise term is: Then the distribution of all noise items of a single noise point is: The noise term distribution of all feature points is: Then Σ is the adaptive matrix R, which plays a role in calculating the Kalman gain.

6. The visual inertial odometer positioning method based on NCC dynamic covariance adjustment according to claim 1, characterized in that: The observation residuals described in step 6 are used to update the state estimation, state correction and error covariance correction to obtain the covariance matrix and gain matrix of the measurement information, so as to obtain more accurate and robust matching results in complex environments where the quality of feature point matching varies significantly; The specific steps are as follows: Step 6-1: In the state prediction step of the Kalman filter, the prior state and error covariance The prediction is made according to the following formula: In the formula, represents the estimated state vector at time t-1, represents the estimated state covariance matrix at time t-1, Q t-1 Represents the covariance matrix corresponding to the system noise, the observation vector Z t With the state vector X t The relationship between the observation model H t describe: Z t =H t X t +v t (24) In the formula, v t represents the observation noise, which is assumed to be zero-mean Gaussian white noise, and the Kalman gain K t The formula is used to weigh the predicted value and the observed value: In the formula, R t Represents the covariance matrix corresponding to the measurement noise, using the observed residual The formulas for updating state estimation, state correction and error covariance correction are as follows: Let α=Σ=diag(Σ i ), then the covariance matrix and gain matrix of the measurement information are:

Images