Monocular vision inertial positioning method based on fine pre-integration and adaptive vision inertial weight

Abstract

The invention discloses a monocular vision inertial positioning method based on fine pre-integration and adaptive vision inertial weight, which comprises the following steps of: firstly, constructing key frames by using parallax by adopting a sliding window method, and processing the output of an inertial measurement unit between adjacent key frames by using fine pre-integration; initializing a visual inertial odometer to obtain external parameters and internal parameters of an initial sensor and scale and speed information of the odometer; normalizing the acceleration and the angular velocity of each pre-integration quantity by adopting an L2 norm, and weighting the components of the covariance matrix constrained by each fine pre-integration by taking the normalized acceleration and angular velocity as adaptive inertia optimization weights; based on the quality of feature matching after optical flow tracking, a visual weight is obtained according to the number of times of feature continuous tracking and changes of gray values before and after the feature continuous tracking, and the size of an information matrix of a visual re-projection residual error is adjusted in a self-adaptive mode; and finally, performing optimization iteration on the camera pose by adopting a sliding window and a factor graph to minimize a residual term based on mahalanobis distance measurement and obtain a final positioning result.

Description

Technical Field

[0001] The invention belongs to the technical field of navigation and positioning, and in particular relates to a monocular visual inertial positioning method based on fine pre-integration and adaptive visual inertial weight. Background Art

[0002] Visual inertial sensors are a very active research area in multi-sensor fusion navigation and positioning because they fuse the inertial measurement unit (IMU) and the image information taken by the camera. Visual information and IMU information are complementary to each other. IMU can assist the scale estimation of monocular cameras and is good at estimating short-term and fast motion states; vision is suitable for calculating long-term and slow motion, and visual information can also be used to estimate the internal parameters of IMU.

[0003] In recent years, many excellent monocular visual inertial positioning algorithms have been proposed, including OKVIS. However, since each iteration process requires recalculating the estimated value of the IMU for the pose, the overall efficiency of the system is low. The current mainstream VINS-Mono algorithm uses the RANSAC algorithm and the basic matrix to eliminate mismatched points. Although it has high accuracy, some mismatches still exist. Not only that, in the visual tracking process, due to various reasons, including changes in light intensity, ambiguity of rapid motion, different exposure times, etc., the quality of each frame image is different. Therefore, according to the effect of optical flow tracking, the weight of visual information should be dynamically adjusted to reduce the influence of outlier measurements in image information to overcome the problem of poor system image quality leading to system performance degradation. At the same time, since the visual inertial combined positioning system usually has four unobservable directions, when the IMU excitation is limited, some parameters may be unobservable. At this time, the processing of inertial information also needs to be changed according to the dynamic characteristics of the carrier. Summary of the invention

[0004] The present invention aims at the problems existing in the prior art and proposes a monocular visual inertial positioning method based on fine pre-integration and adaptive visual inertial weight. Firstly, a sliding window method is adopted to construct key frames using parallax, and the output of the inertial measurement unit between adjacent key frames is processed by fine pre-integration means; then the visual inertial odometer is initialized to obtain the initial external parameters, internal parameters of the sensor and the scale and speed information of the odometer; the acceleration and angular velocity of each pre-integrated quantity are normalized by L2 norm, and are used as adaptive inertial optimization weights to weight the components of the covariance matrix of each fine pre-integration constraint; at the same time, based on the quality of feature matching after optical flow tracking, the visual weight is obtained according to the number of continuous feature tracking and the size of the gray value change before and after, and the size of the information matrix of the visual reprojection residual is adaptively adjusted; finally, the sliding window and factor graph are used to optimize and iterate the camera posture, so that the residual term based on the Mahalanobis distance measurement is minimized, the optimal motion trajectory of the carrier is obtained, and the positioning result is obtained. The method of the present invention can more accurately and effectively fuse the information of each sensor, improve the overall robustness and accuracy of the navigation system, and more reasonably fuse the multi-sensor information through adaptive vision and inertial weights to improve the accuracy of posture estimation.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is: a monocular visual inertial positioning method based on fine pre-integration and adaptive visual inertial weight, comprising the following steps:

[0006] S1. Data preprocessing: Using the sliding window method, the parallax is used to construct key frames, and the output of the inertial measurement unit between adjacent key frames is processed using a fine pre-integration method;

[0007] S2. Visual inertial odometer initialization: Initialize the visual inertial odometer to obtain the initial sensor external parameters, internal parameters, and odometer scale and speed information;

[0008] S3, adaptive fine pre-integration covariance matrix adjustment: use L2 norm to normalize the acceleration and angular velocity of each pre-integration component, and use it as adaptive inertia optimization weight to weight the components of the covariance matrix of each fine pre-integration constraint;

[0009] S4, adaptive visual reprojection residual adjustment: Based on the quality of feature matching after optical flow tracking, the visual weight is obtained according to the number of continuous feature tracking and the size of the gray value change before and after, and the size of the information matrix of the visual reprojection residual is adaptively adjusted;

[0010] S5. Obtaining the optimal motion trajectory: Use the sliding window and factor graph to iteratively optimize the camera pose so that the residual term based on the Mahalanobis distance measurement is minimized to obtain the optimal motion trajectory of the carrier.

[0011] As an improvement of the present invention, step S1 specifically includes the following steps:

[0012] S11: For the input image, the optical flow method is used to track the image feature points of the front end, and the detected feature points are added to the container, and the number of tracking times of each point and the corresponding disparity of the feature point pair are calculated;

[0013] S12: Detect the disparity between the two frames and determine whether the next latest frame is a key frame; the determination method is specifically: if the feature average disparity of the current frame relative to the most recent key frame is greater than a threshold, and the number of feature points tracked by the current frame is less than the threshold, it is regarded as a key frame and stored in the sliding window;

[0014] S13: For the output of the inertial measurement unit, a fine pre-integration method is used to align it with the visual information to obtain the position, velocity, and attitude changes between each key frame.

[0015] As another improvement of the present invention, the fine pre-integration method in step S13 is specifically as follows:

[0016] Using the kinematic equation, the carrier system is the b system and the world coordinate system is the w system, the position p between two adjacent frames is obtained w , speed v w , posture q w The update equation is:

[0017]

[0018] Among them, Δt k is the time interval, g w is the gravity vector in the world coordinate system, a and ω are the outputs directly measured by the accelerometer and gyroscope, and ε g are the zero bias of the accelerometer and gyroscope, n a and n g is the measurement noise of the accelerometer and gyroscope, T a and T g Parameters related to the scale factor and installation error of the gyroscope and accelerometer, is the rotation matrix from the b system to the w system at time t, and the reference system is transferred from the world coordinate system to the b system, and we get:

[0019]

[0020] At this time, the pre-integrated position change Speed ​​change Posture change for:

[0021]

[0022] As another improvement of the present invention, step S2 specifically includes the following steps:

[0023] S21: Recover the pose rotation matrix between cameras through the 2D-2D essential matrix of vision, and the rotation matrix between IMUs calculated by the fine IMU pre-integration method, and construct an over-constrained linear equation with robust kernel weights to obtain the relative external parameters between the camera and IMU:;

[0024] S22: Use motion recovery SFM to restore the pose of all frames in the sliding window:

[0025]

[0026]

[0027] in is the rotation matrix between the carrier coordinate system and the camera coordinate system, c0 is the reference frame of SFM, c k and b k are the camera coordinate system and carrier coordinate system at the kth key frame, is the translation between the carrier coordinate system and the camera coordinate system, and s is the scale factor;

[0028] S23: Align the visual information and the inertial information, construct an objective function for correcting the gyroscope bias and minimize the objective function, thereby completing the correction of the gyroscope bias;

[0029]

[0030]

[0031] where b k and b k+1 are the key frames at time k and k+1, B represents all frames in the sliding window, and is the rotation between two adjacent frames, is the attitude change between the two key frame body coordinate systems obtained by fine IMU pre-integration, ε g represents the desired gyroscope bias, represents the first-order approximation of the pre-integrated gyroscope bias error;

[0032] S24: Match the fine IMU pre-integration result with the point structure without scale factor to obtain the initial quantities of velocity, gravitational acceleration and scale factor.

[0033] As another improvement of the present invention, in step S3, for each pre-integrated quantity in the sliding window, the average output of the inertial measurement unit between each sliding window is obtained, and the L2 norm is taken. For the output directly measured by the input accelerometer and gyroscope, after deducting the zero bias, the maximum and minimum values ​​of the average acceleration and the average angular velocity are recorded max(||ag w ||2),min(||ag w ||2),max||ω||2,min||ω||2, perform normalization to obtain the adaptive optimization weight μ of inertial information A and μ G :

[0034]

[0035]

[0036] As another improvement of the present invention, in step S4, for the visual reprojection error of each matching point pair, the magnitude ΔI of the grayscale value on the two frames of images before and after each feature matching is obtained:

[0037] ΔI=|Ι i -Ι j |

[0038] Among them, I i and I j are the grayscale values ​​of the feature point in the i-th frame and the j-th frame respectively, and the number of times the feature point is tracked N is counted. The visual adaptive weight θ is:

[0039]

[0040] Among them, σ, χ are constant coefficients, θ threshold is the weight threshold.

[0041] As a further improvement of the present invention, in step S5, a factor graph is used to optimize the camera pose in the sliding window so as to minimize the measurement residual term, and the residual term is based on the Mahalanobis distance:

[0042]

[0043] where ||r p -H p X|| 2 represents the marginalization error, is the adaptive fine pre-integrated measurement residual, Represents the adaptive visual reprojection error, and the state variable X to be optimized is:

[0044] X=[x0,x1,...,x n ,λ0,λ1,...,λm ]

[0045]

[0046] Among them, λ is the inverse depth of each feature point. The optimization problem is constructed using the ceres library. After multiple iterations, the residual term based on the Mahalanobis distance measurement is minimized. The optimal motion trajectory of the carrier is obtained by joint optimization to obtain the final positioning result.

[0047] Compared with the prior art, the present invention has the following beneficial effects: the present invention proposes a monocular visual-inertial positioning method based on fine pre-integration and adaptive visual-inertial weight, which first uses fine pre-integration to obtain more accurate posture transformation, and at the same time optimizes the inertial visual factor graph, and uses adaptive visual weight to reduce the influence of external bad light intensity on posture solution, so that the adaptive visual weight decreases with the deterioration of image quality; at the same time, during small maneuvers, the adaptive inertial weight decreases with the decrease of the signal-to-noise ratio of the inertial measurement unit. This method can more reasonably fuse multi-sensor information, improve the accuracy of posture estimation, and obtain more accurate positioning results. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 The present invention is a flowchart of the steps of the monocular visual inertial positioning method based on fine pre-integration and adaptive visual inertial weight;

[0049] Figure 2 This is a schematic diagram of the adaptive inertia weight change of the MH_01 sequence on the EuRoC data set in the test example of the present invention;

[0050] Figure 3 This is a schematic diagram of the adaptive visual weight change of the MH_05 sequence on the EuRoC dataset in the test example of the present invention;

[0051] Figure 4 FIG. 1 is a schematic diagram comparing the absolute trajectory errors of VINS-Mono and the method of the present invention in the test example of the present invention, wherein:

[0052] (a) Schematic diagram of the absolute trajectory error of VINS-Mono under sequence MH_01;

[0053] (b) is a schematic diagram of the absolute trajectory error of VINS-Mono under sequence MH_03;

[0054] (c) is a schematic diagram of the absolute trajectory error of VINS-Mono under sequence MH_05;

[0055] (d) is a schematic diagram of the absolute trajectory error of the method of the present invention under the sequence MH_01;

[0056] (e) is a schematic diagram of the absolute trajectory error of the method of the present invention under sequence MH_03;

[0057] (f) is a schematic diagram of the absolute trajectory error of the method of the present invention under sequence MH_05. DETAILED DESCRIPTION

[0058] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0059] Example 1

[0060] like Figure 1 As shown in the figure, a monocular visual inertial positioning method based on fine pre-integration and adaptive visual inertial weight is proposed. The specific steps are as follows:

[0061] Step S1: Using a sliding window approach, key frames are constructed using parallax, and the output of the inertial measurement unit between adjacent key frames is processed using a fine pre-integration approach.

[0062] For the input image, the optical flow method is used to track the image feature points at the front end, and the detected feature points are added to the container. The number of times each point is tracked and the corresponding disparity of the feature points are calculated. Then, the disparity between the two frames is detected to determine whether the next-newest frame is a key frame. If the average feature disparity of the current frame relative to the most recent key frame is greater than a threshold and the number of feature points tracked by the current frame is less than the threshold, it is regarded as a key frame and stored in a sliding window. Then, for the output of the inertial measurement unit, a fine pre-integration method is used to align it with the visual information to obtain the position, velocity, and posture changes between each key frame.

[0063] In this embodiment, the size of the input image is 480×752, the frequency of the IMU is 200 Hz, the camera model uses a pinhole model, the size of the sliding window is selected as 10, the LK optical flow method is used to track the Harris corner points of the front-end image, and the detected feature points are added to the container.

[0064] The specific method of fine IMU pre-integration is: use the kinematic equation, with the carrier system as the b system and the world coordinate system as the w system to obtain the position p between two adjacent frames w , speed v w , posture q w The update equation is:

[0065]

[0066] Among them, Δt k is the time interval, g wis the gravity vector in the world coordinate system, a and ω are the outputs directly measured by the accelerometer and gyroscope, and ε g are the zero bias of the accelerometer and gyroscope, n a and n g is the measurement noise of the accelerometer and gyroscope, T a and T g Parameters related to the scale factor and installation error of the gyroscope and accelerometer, is the rotation matrix from the b system to the w system at time t, and the reference system is transferred from the world coordinate system to the b system, and we get:

[0067]

[0068] At this time, the pre-integrated position change Speed ​​change Posture change for:

[0069]

[0070] Step S2: Initialize the visual inertial odometer to obtain the initial sensor external parameters, internal parameters, and scale and speed information of the odometer.

[0071] The pose rotation matrix between cameras is restored through the visual 2D-2D essential matrix, and the rotation matrix between IMUs is calculated through the fine IMU pre-integration method, and an over-constrained linear equation with robust kernel weights is constructed to obtain the relative extrinsic parameters between the camera and IMU.

[0072] Then use the motion recovery structure SFM to recover the poses of all frames in the sliding window.

[0073]

[0074]

[0075] in is the rotation matrix between the carrier coordinate system and the camera coordinate system, c0 is the reference frame of SFM, c k and b k are the camera coordinate system and carrier coordinate system at the kth key frame, is the translation between the carrier coordinate system and the camera coordinate system, and s is the scale factor.

[0076] The next step is to align the visual information and inertial information, construct an objective function for correcting the gyroscope bias and minimize the objective function, thereby completing the correction of the gyroscope bias.

[0077]

[0078]

[0079] where b k and b k+1 are the key frames at time k and k+1, B represents all frames in the sliding window, and is the rotation between two adjacent frames, is the attitude change between the two key frame body coordinate systems obtained by fine IMU pre-integration, ε g represents the desired gyroscope bias, represents the first-order approximation of the pre-integrated gyroscope bias error.

[0080] Finally, the fine IMU pre-integration results are matched with the point structure without scale factor to obtain initial quantities such as velocity, gravitational acceleration and scale factor.

[0081] Step S3: normalize the acceleration and angular velocity of each pre-integrated component using the L2 norm, and use it as the adaptive inertia optimization weight to weight the components of the covariance matrix of each fine pre-integrated constraint.

[0082] For each pre-integrated component in the sliding window, the average output of the inertial measurement unit between each sliding window is obtained, and the L2 norm is taken. For the output obtained by direct measurement of the input accelerometer and gyroscope, after deducting the zero bias, the maximum and minimum values ​​of the average acceleration and the average angular velocity are recorded (||ag w ||2),min(||ag w ||2),max||ω||2,min||ω||2, perform normalization to obtain the adaptive optimization weight μ of inertial information A and μ G .

[0083] In the static state, the outputs of the accelerometer and gyroscope are close to gravity acceleration and zero, respectively. At this time, the signal-to-noise ratio of the sensor is low and the useful information is small. In the process of system solution, it is necessary to reduce the optimization amplitude of the acceleration zero bias and the gyroscope zero bias, so that μ A and μ G When the carrier moves with a large amplitude, the sensor is fully stimulated and the observability of the system state is large, so μ A and μ G Become larger to increase the optimization range of acceleration bias and gyroscope bias to obtain more accurate positioning of the system.

[0084]

[0085]

[0086] For each pre-integrated quantity, its corresponding covariance Adjust to

[0087]

[0088] Where μ = diag(I 9*9 μ A I 3*3 μ G I 3*3 μ A I 6*6 μ A I 9*9 ), and then Do LLT decomposition so that r B is the measurement residual of the fine pre-integration, then the Mahalanobis distance of the residual of the fine pre-integration component to be optimized can be written as:

[0089]

[0090] Step S4: Based on the quality of feature matching after optical flow tracking, the visual weight is obtained according to the number of continuous feature tracking and the size of the grayscale value change before and after, and the size of the information matrix of the visual reprojection residual is adaptively adjusted.

[0091] For the visual reprojection error of each matching point pair, first obtain the grayscale value ΔI of each feature before and after the two frames of image matching:

[0092] ΔI=|Ι i -Ι j

[0093] I i and I j are the grayscale values ​​of the feature point in the i-th frame and the j-th frame respectively, and the number of times the feature point is tracked N is counted. The visual adaptive weight θ is:

[0094]

[0095] Among them, σ, χ are constant coefficients, θ threshold is the weight threshold, which is set comprehensively according to the scene environment factors. In this embodiment, it can be set to 1.4. The adaptive information matrix of visual constraints can be written as ∑′ vis_Info ,∑ vis_InfoThe original information matrix represents the visual constraint, f is the camera focal length, which is 460, and the noise is generally selected to be 1.5 pixels. When the image quality deteriorates (including image blur, drastic changes in front and back lighting, and few feature point tracking times), the size of the information matrix is ​​reduced, so that it is less biased towards optimizing the visual reprojection residual term. When the image is stable and the carrier motion slows down, the visual adaptive weight θ can be increased within the threshold range to make the system more trustful of the visual information at this time.

[0096] ∑′ vis_Info =θ*∑ vis_Info

[0097]

[0098] Step S5: Use the sliding window and factor graph to iteratively optimize the camera pose so as to minimize the residual term based on the Mahalanobis distance measurement and obtain the optimal motion trajectory of the carrier.

[0099] The factor graph is used to optimize the camera pose within the sliding window to minimize the measurement residual term, which is based on the Mahalanobis distance:

[0100]

[0101] where ||r p -H p X|| 2 represents the marginalization error, is the pre-integrated measurement residual, Represents the adaptive visual reprojection error, and the state variable X to be optimized is:

[0102] X=[x0,x1,...,x n ,λ0,λ1,...,λ m ]

[0103]

[0104] λ is the inverse depth of each feature point. The ceres library is used to construct the optimization problem, and the corresponding Jacobian matrix, state variables to be optimized, and residual blocks are added. The maximum number of iterations is set, and multiple iterations are performed to minimize the residual term based on the Mahalanobis distance measurement. The optimal motion trajectory of the carrier is obtained by joint optimization, and the final positioning result is obtained.

[0105] Test Case

[0106] A monocular visual-inertial positioning method based on adaptive visual and inertial weights. The experimental simulation environment of this test case is: CPU Intel core i5-8400H, Ubuntu 18.04 LTS, Ros melodic.

[0107] The public dataset EuRoC MAV is selected for simulation. For the EuRoC MAV dataset, the factory MachineHall sequence is selected for evaluation, which includes 5 sequences with complex lighting conditions, simple textures, and diverse motion conditions.

[0108] In order to verify the performance of the method of the present invention, the MH_01~MH_05 subsequences in the EuRoC dataset were selected to compare VINS-Mono and the method of the present invention. The test results are shown in Figure 2 to Figure 4 shown.

[0109] Figure 2 (a) shows the change of adaptive inertial weight of MH_01 sequence on EuRoC dataset. In the first 10 to 35 seconds, the UAV in EuRoC is in the take-off stage, the output of accelerometer is sufficient, and the proportion of sensor noise information is small, which makes μ A Larger, in order to increase the magnitude of optimization of accelerometer related parameters, and in the following time, when the accelerometer is insufficiently excited, μ A Become smaller accordingly. Figure 2 (b) shows its μ G The change in amplitude shows that in the initial takeoff stage and μ A With similar results, the adaptive inertia weight μ is obtained by further varying the amplitude of the carrier at the next time. G The value of .

[0110] Figure 3 The changes in the adaptive visual weights of the MH_05 sequence on the EuRoC dataset are shown. Since there are many pairs of feature point matches on an image, the weight in this test case is the average weight of all matching pairs at the same time. It can be seen that the average weight is between 0.6 and 1.4, and the information optimization amplitude of the reprojected visual residual of each matching point is adjusted accordingly to obtain the solution of the optimal trajectory.

[0111] Figure 4 The absolute trajectory error diagram of VINS-Mono and the method of the present invention is shown, where (a), (b), and (c) represent the absolute trajectory errors of VINS-Mono in sequences MH_01, MH_03, and MH_05, respectively, and (d), (e), and (f) represent the absolute trajectory errors of the method of the present invention in the corresponding sequences, respectively. It can be clearly seen from the figure that no matter which subsequence of the data set, the absolute trajectory error of the method of the present invention is smaller.

[0112] Table 1 shows the root mean square error (RMSE) of the absolute trajectory error (ATE) of VINS-Mono and the method of the present invention. The quantitative comparison results show that the present invention improves the positioning accuracy of the visual inertial positioning system in various motion and lighting environments.

[0113] Table 1 RMSE (m) of absolute trajectory error of VINS-Mono and the method of the present invention

[0114] Sequence VINS-Mono Method of the present invention Improved accuracy MH_01 0.180 0.166 7.78％ MH_02 0.179 0.167 6.70％ MH_03 0.236 0.153 35.17％ MH_04 0.314 0.313 0.32％ MH_05 0.320 0.287 10.31％

[0115] It can be seen from this that the present invention designs a monocular visual inertial positioning method based on fine pre-integration and adaptive visual inertial weights, uses fine pre-integration and constructs adaptive visual weights and adaptive inertial visual weights, and dynamically adjusts the information matrix and covariance matrix of the visual information and inertial information in the combined navigation and positioning system through the size changes of the grayscale values ​​before and after feature matching in the scene, the number of times the feature points are tracked, and the acceleration and angular velocity of each pre-integrated quantity, so as to improve the weight ratio of the factor graph optimization solution. In the case of a decrease in the optical flow tracking effect of the front-end visual image in scenes with rapid motion and lighting changes, poor matching quality of feature points, and insufficient IMU excitation, the visual and inertial weights are adjusted in real time according to the effect of feature point tracking, so as to achieve more effective information fusion and improve the positioning accuracy and robustness of the visual inertial positioning system in a variety of complex environments.

[0116] It should be noted that the above content only illustrates the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications all fall within the protection scope of the claims of the present invention.

Claims

1. A monocular visual-inertial positioning method based on fine pre-integration and adaptive visual-inertial weighting, characterized in that: The steps include: S1. Data preprocessing: Using the sliding window method, the parallax is used to construct key frames, and the output of the inertial measurement unit between adjacent key frames is processed using a fine pre-integration method; S2. Visual inertial odometer initialization: Initialize the visual inertial odometer to obtain the initial sensor external parameters, internal parameters, and scale and speed information of the odometer; S3, adaptive fine pre-integration covariance matrix adjustment: use L2 norm to normalize the acceleration and angular velocity of each pre-integration component, and use it as adaptive inertia optimization weight to weight the components of the covariance matrix of each fine pre-integration constraint; S4, adaptive visual reprojection residual adjustment: Based on the quality of feature matching after optical flow tracking, the visual weight is obtained according to the number of continuous feature tracking and the size of the gray value change before and after, and the size of the information matrix of the visual reprojection residual is adaptively adjusted; S5. Obtaining the optimal motion trajectory: Use the sliding window and factor graph to iteratively optimize the camera pose so that the residual term based on the Mahalanobis distance measurement is minimized to obtain the optimal motion trajectory of the carrier.

2. The monocular visual-inertial positioning method based on fine pre-integration and adaptive visual-inertial weighting as claimed in claim 1, characterized in that: The step S1 specifically includes the following steps: S11: For the input image, the optical flow method is used to track the image feature points of the front end, and the detected feature points are added to the container, and the number of tracking times of each point and the corresponding disparity of the feature point pair are calculated; S12: Detect the disparity between the two frames and determine whether the next latest frame is a key frame; the determination method is specifically: if the feature average disparity of the current frame relative to the most recent key frame is greater than a threshold, and the number of feature points tracked by the current frame is less than the threshold, it is regarded as a key frame and stored in the sliding window; S13: For the output of the inertial measurement unit, a fine pre-integration method is used to align it with the visual information to obtain the position, velocity, and attitude changes between each key frame.

3. The monocular visual-inertial positioning method based on fine pre-integration and adaptive visual-inertial weighting as claimed in claim 2, characterized in that: The detailed pre-integration method in step S13 is as follows: Using the kinematic equation, the carrier system is the b system and the world coordinate system is the w system, the position p between two adjacent frames is obtained w , speed v w , posture q w The update equation is: Among them, Δt k is the time interval, g w is the gravity vector in the world coordinate system, a and ω are the outputs directly measured by the accelerometer and gyroscope, ▽ a and ε g are the zero bias of the accelerometer and gyroscope, n a and n g is the measurement noise of the accelerometer and gyroscope, T a and T g Parameters related to the scale factor and installation error of the gyroscope and accelerometer, is the rotation matrix from the b system to the w system at time t, and the reference system is transferred from the world coordinate system to the b system, and we get: At this time, the pre-integrated position change Speed ​​change Posture change for:

4. The monocular visual-inertial positioning method based on fine pre-integration and adaptive visual-inertial weighting as claimed in claim 1, characterized in that: The step S2 specifically includes the following steps: S21: Recover the pose rotation matrix between cameras through the 2D-2D essential matrix of vision, and the rotation matrix between IMUs calculated by the fine IMU pre-integration method, and construct an over-constrained linear equation with robust kernel weights to obtain the relative external parameters between the camera and IMU; S22: Use motion recovery SFM to restore the pose of all frames in the sliding window: in is the rotation matrix between the carrier coordinate system and the camera coordinate system, c0 is the reference frame of SFM, c k and b k are the camera coordinate system and carrier coordinate system at the kth key frame, is the translation between the carrier coordinate system and the camera coordinate system, and s is the scale factor; S23: Align the visual information and the inertial information, construct an objective function for correcting the gyroscope bias and minimize the objective function, thereby completing the correction of the gyroscope bias; where b k and b k+1 are the key frames at time k and k+1, B represents all frames in the sliding window, and is the rotation between two adjacent frames, is the attitude change between the two key frame body coordinate systems obtained by fine IMU pre-integration, ε g represents the desired gyroscope bias, represents the first-order approximation of the pre-integrated gyroscope bias error; S24: Match the fine IMU pre-integration result with the point structure without scale factor to obtain the initial quantities of velocity, gravitational acceleration and scale factor.

5. The monocular visual-inertial positioning method based on fine pre-integration and adaptive visual-inertial weighting as claimed in claim 1, characterized in that: In step S3, for each pre-integrated component in the sliding window, the average output of the inertial measurement unit between each sliding window is obtained, and the L2 norm is taken. For the output directly measured by the input accelerometer and gyroscope, after deducting the zero bias, the maximum and minimum values ​​of the average acceleration and the average angular velocity are recorded (||ag w ||2),min(||ag w ||2),max||ω||2,min||ω||2, perform normalization to obtain the adaptive optimization weight μ of inertial information A and μ G :

6. The monocular visual-inertial positioning method based on fine pre-integration and adaptive visual-inertial weighting as claimed in claim 1, characterized in that: In step S4, for the visual reprojection error of each matching point pair, the magnitude ΔI of the grayscale value on the two frames of images before and after each feature matching is obtained: ΔI=|I i -I j | Among them, I i and I j are the grayscale values ​​of the feature point in the i-th frame and the j-th frame respectively, and the number of times the feature point is tracked N is counted. The visual adaptive weight θ is: Among them, σ, χ are constant coefficients, θ threshold is the weight threshold.

7. The monocular visual-inertial positioning method based on fine pre-integration and adaptive visual-inertial weighting as claimed in claim 1, characterized in that: In step S5, the factor graph is used to optimize the camera pose in the sliding window so as to minimize the measurement residual term, and the residual term is based on the Mahalanobis distance: where ||r p -H p X|| 2 represents the marginalization error, is the adaptive fine pre-integrated measurement residual, Represents the adaptive visual reprojection error, and the state variable X to be optimized is: X=[x0,x1,...,x n ,λ0,λ1,...,λ m ] Among them, λ is the inverse depth of each feature point. The optimization problem is constructed using the ceres library. After multiple iterations, the residual term based on the Mahalanobis distance measurement is minimized. The optimal motion trajectory of the carrier is obtained by joint optimization to obtain the final positioning result.

Images