Monocular vision inertial fusion positioning method based on optical flow smoothness constraint

Abstract

The invention discloses a monocular vision inertial fusion positioning method based on an optical flow smoothness constraint, which comprises the following steps: designing a visual feature enhancement algorithm based on the optical flow smoothness constraint, improving feature point tracking and elimination in a visual inertial navigation system, and carrying out tight coupling optimization on a visual residual error part so as to improve the quality of observed quantity in positioning; comprising the following steps: generating a clustering result and a support factor for each feature point tracking vector of an acquired image through optical flow smoothness constraint; constructing a compound expectation maximization (EM) algorithm, fusing a support factor into algorithm solution to serve as prior information, and eliminating wrong matching point pairs to obtain a correct matching vector; adaptively adjusting the visual weight of the pose estimator, and performing region shielding; and monocular vision inertial fusion positioning based on optical flow smoothness constraint is realized.

Description

Technical Field

[0001] This invention belongs to the field of visual inertial odometry (VIO, visual inertial navigation) technology, specifically relating to a monocular visual-inertial fusion positioning method based on optical flow smoothness constraints in a GNSS-denied environment. Background Technology

[0002] With the rapid development of technologies such as autonomous driving and intelligent robots, the demand for high-precision, high-reliability, real-time, low-cost positioning systems has increased unprecedentedly. Traditional positioning methods typically rely on global navigation satellite systems, which have high requirements for signal reception and are prone to failure in situations of weak or denied satellite observation. Visual sensors and micro-inertial devices are small in size, low in cost, and low in power consumption, and completely independent of external facilities. Visual-inertial integrated systems can fully utilize the excellent complementary characteristics between sensors, and have great potential for completing high-precision and high-reliability navigation tasks. They have significant research value and application needs, and are gradually becoming a research hotspot in this field.

[0003] Based on different pose estimation methods, existing visual-inertial fusion localization algorithms can be divided into filtering-based methods and optimization-based methods. Filtering-based methods estimate and correct the quantities to be estimated in the next state based on the previous state, typically using improvements to the Kalman filter (KF), and were an early method for visual-inertial localization. Optimization-based methods, also known as bundle adjustment (BA), usually treat inertial data as prior or regularization terms, relying on image feature extraction matching and post-processing for optimization. This method optimizes the camera pose and the 3D coordinates of feature points by minimizing the reprojection error of the same feature point between different image frames, i.e., the distance between the position of the feature point projected onto the imaging plane in the world coordinate system using bundle or collinearity equations and the actual position of the image point obtained from the capture.

[0004] Because filtering methods are based on the first-order Markov assumption, they are not well-suited for handling data correlation problems, and linearization approximation errors lead to low prediction accuracy. Nonlinear optimization methods perform a second linearization process on past measurement results, achieving more accurate state estimation. However, iterative optimization methods consume more computational resources, typically sacrificing some accuracy to ensure real-time performance by reducing the number of variables to be optimized.

[0005] The lighting conditions of real-time data acquired by visual sensors in natural environments are unstable, and the visual images contain many dynamic objects and textureless regions, resulting in poorer feature point extraction and tracking performance. Secondly, due to the computational limitations of mobile vehicles, optimization frames typically span multiple frames of actually acquired images, while feature optical flow tracking and outlier removal are based on continuous images. When the frequency of real-time data is higher than that of the open-source dataset used in testing, image parallax is smaller, and small tracking errors are difficult to eliminate by general outlier detection algorithms, leading to offsets in feature tracking positions. Finally, most existing local optimization frameworks in VIO (Visual-Inertial Odometry) systems include three parts: marginalized prior residuals, pre-integrated residuals from the inertial measurement unit (IMU) in the VIO system, and visual reprojection residuals. They typically express the minimization objective function as the sum of these three components without considering their relative importance, thus still having certain limitations. Summary of the Invention

[0006] To overcome the shortcomings of the existing technology, this invention provides a monocular vision-inertial fusion localization method based on optical flow smoothness constraints, which can improve the prediction accuracy of the VIO system and enable the VIO system to achieve higher localization accuracy and stronger robustness in more challenging unstructured scenarios.

[0007] This invention improves feature point tracking and removal in VIO visual inertial navigation systems by designing a visual feature enhancement algorithm based on optical flow smoothness constraints, and by tightly coupling and optimizing the visual residual part. This prevents feature point tracking drift from being mistakenly retained during high-frequency acquisition of sensor data in VIO systems, while also mitigating the impact of lack of texture in sensor-acquired image data and interference from moving objects on VIO systems.

[0008] The specific implementation steps of the monocular vision-inertial fusion localization method based on optical flow smoothness constraints of this invention are as follows:

[0009] Step 1: Acquire visual images and IMU data;

[0010] Based on the IMU data, preliminary estimates of the object's position, velocity, and rotation are obtained. The IMU pre-integration factor in the factor graph optimization is obtained by accumulating the IMU measurement increment between image frames through pre-integration. The factors in the factor graph represent the observations in the state estimation problem, and the nodes represent the state variables to be estimated.

[0011] For image data, feature points are extracted and tracked between images. The tracked same feature point is reprojected from the previous frame to the next frame using the estimated pose transformation between the two frames to obtain the visual feature reprojection factor in factor map optimization. Step 2 involves designing a visual feature enhancement algorithm based on a smooth optical flow field to enhance the visual image data and obtain the visual feature point support factor. The support factor is a weighted coefficient characterizing the quality of the feature point. Mismatched feature point pairs are removed using the EM algorithm, and unstructured region feature points are clustered and masked using the support factor. The visual reprojection factor is then weighted. The process includes the following steps:

[0012] Step 1: For the visual images acquired by the sensor in Step 1, establish an optical flow field model, and generate clustering results and support factors by tracking the vector of each image feature point through optical flow smoothness constraints.

[0013] First, an optical flow field model is established. It is assumed that a set of image feature point tracking vectors, with N vectors, has been obtained between the two latest consecutive optimized frames within the sliding window of the GNSS pose estimator. Then, x is used... i With y i Let m represent the normalized image pixel coordinates of the i-th pair of successfully tracked feature points. Then, the displacement vector determined by the relative positional relationship of this pair of feature points can be expressed as m. i =y i -x i Let M represent the set of all displacement vectors of the tracking feature points. The objective is to recover the optical flow field between two visual images using this set of image feature point tracking vectors. To ensure the smoothness of the optical flow field, the optical flow field... The solution can be expressed as a least squares problem as shown in Equation 1, where regularization theory is used to further constrain the problem. Solve for it.

[0014]

[0015] Here, λ is a regularization parameter used to balance error and solution complexity. The optimal solution is shown in Equation 2.

[0016]

[0017] Where x is the normalized image pixel coordinate; Let be the optical flow field, i.e., the optical flow vector representing the optical flow displacement between two frames at any pixel x; t1 and t2 are N×1 column vectors composed of real coefficients, representing the contribution weights of feature points to the two-dimensional components of the optical flow field, with the superscript indicating the i-th element of the vector, corresponding to the i-th feature point; R is the kernel function, an N×N matrix, representing the correlation between every two features. Then the optimization problem in Equation 1 can be rewritten as Equation 3.

[0018]

[0019] Here, M1 and M2 represent the x-axis and y-axis components of each displacement vector in set M, forming an N×1 column vector. Therefore, the optical flow coefficients can be obtained by solving the linear system shown in Equation 4.

[0020]

[0021] Since image capture is a continuous process, it is easy to see that the displacement vector and its neighboring vectors follow a smoothness constraint, meaning that their lengths and directions generally tend to be similar. The matrix S used to quantify the consistency support relationship between two vectors is calculated as Equation 5.

[0022]

[0023] Among them, the consistency support factor S i,j The value increases with the higher the consistency between vectors in the range [-1, 1]. It's worth noting that when the drone is stationary, the tracking displacement of tiny feature points often exhibits random direction. In this case, the calculated cosine similarity cannot accurately reflect the tracking quality of the feature points. Therefore, in actual operation, a vector length judgment is performed, and when the displacement is less than a threshold, the cosine similarity is not included in the support matrix calculation.

[0024] In some complex scenarios, there are large dynamic objects and textureless regions in the image, which can lead to local smoothing phenomena between some clustered mismatched vectors. Neighborhood constraints cannot identify these outliers. Therefore, in addition to the non-free change characteristics of the local neighborhood structure of vectors due to their physical constraints, it is also necessary to constrain the distribution of vectors to conform to the smooth and slow motion field structure of global consensus.

[0025] Define the distance matrix D between two displacement vectors as shown in Equation 6, considering the difference between the absolute position distance of the vectors in the image and the vectors themselves.

[0026] D i,j =d E (c i ,c j )+ω i,j ·d E (m i ,m j (6)

[0027] Among them, D i,j d is the distance matrix; E (·) represents the Euclidean distance between two-dimensional vectors. i Calculate the center position coordinates of the displacement vector, ω i,jThe weight of the displacement vector difference is defined by Formula 7, which assumes that the greater the relative position distance, the weaker the constraint on the consistency of motion of the vector.

[0028]

[0029] Here, the computational cost of D is negatively correlated with the actual correlation of the vectors. Normalizing it to between 0 and 1 using Formula 8, we can express the correlation coefficient matrix R between the displacement vectors.

[0030]

[0031] Where μ represents the distance between vectors, i.e., the mean of matrix D. R is the kernel function in Equation 2.

[0032] The consistency support factor and correlation coefficient between every two vectors are calculated using formulas 5 and 8. The global support factor of a vector is obtained by multiplying each support factor and correlation coefficient associated with the vector and summing the results. Correct feature point tracking tends to yield a larger global support factor. However, due to image boundedness, vectors closer to the edge receive less support than those at the center. A simplified correction amount, as shown in formula 9, is defined to eliminate edge degradation inherent in image location.

[0033]

[0034] in, c is the amount of correction for edge degeneration. i This still represents the coordinate mean, with the superscripts a and b indicating the components on the x and y axes, respectively, i.e., c. i The first and second dimensions.

[0035] Next, clustering is performed on the displacement vector set. The column sum or row sum of the correlation coefficient matrix can represent the density ρ of each vector; that is, more vectors provide higher correlation, and the density is greater. Note that the density calculation also needs to be edge-corrected using Equation 9. At the same time, since density calculations often produce consecutive extrema in a dense vector region, leading to clustering of cluster centers, a secondary adjustment to the ρ value is made as shown in Equation 10 to ensure a uniform distribution of cluster centers.

[0036]

[0037] Here, μ represents the mean of matrix D. Cluster centers are first selected based on density sorting, and then vectors are grouped into clusters with their nearest neighbors (vectors with higher density) to complete the clustering process.

[0038] To increase the significance of differences between vectors and improve the robustness of the support factor, in addition to the global vector consistency constraint, the support of the cluster center vector of the vector is added to the calculation of the γ factor according to the above clustering results. Finally, the support factor for each displacement vector can be expressed as Equation 11.

[0039]

[0040] Where the subscript ci represents the center vector number of the cluster to which the i-th vector belongs, and n represents the expression that satisfies D i,j The number of vectors ≥ μ. α and β are the global support factor and the proportional coefficients of cluster support, respectively.

[0041] The second step is to construct a complex EM algorithm (Expectation-Maximization algorithm), which integrates support factors into the solution of the complex EM algorithm as prior information, and removes incorrect matching point pairs to obtain the correct matching vector.

[0042] A matching point pair refers to the same feature point observed in two frames of images. When matching feature points in two frames of images, mismatches may occur, where feature points that are clearly not on the same object are matched.

[0043] Using random variable z i ∈{0,1} indicates whether the feature points match correctly, then z i The distribution of follows a binomial distribution, i.e., P(Z) i =1)=ξ,P(z i =0) =1-ξ, where ξ represents the probability of a correct feature point match, defined by Equation 12. When z i =1 indicates that the i-th vector is correctly tracked. It is assumed that the noise is independent in both the horizontal and vertical directions and follows a zero-mean Gaussian distribution. Here, the superscripts a and b represent the components on the x and y coordinates, respectively, and σ represents the standard deviation of the Gaussian distribution. The mismatch vector noise follows a uniform distribution across the entire image. The likelihood function P can be defined as Equation 12.

[0044]

[0045] in The unknown parameters of the optical flow field are represented by X, which is the set of pixel coordinates of feature points, M is the set of displacement vectors of each feature point, and the subscript i represents the index number of the feature point.

[0046] The support factors obtained in the previous step are incorporated into the EM algorithm as prior information to help the EM algorithm converge faster. The binomial distribution parameters for correct and incorrect matching are defined as Equation 13. When a displacement vector receives high support, the probability of mismatch is lower, and the Gaussian distribution in Equation 12 has a larger proportion in the complex model. Conversely, the uniform distribution corresponding to incorrect matching has a larger proportion, which strengthens the model's ability to distinguish whether feature point tracking is correct.

[0047]

[0048] Where k and b represent the transformation coefficients from support factors to distribution parameters.

[0049] The Expectation-Maximization (EM) algorithm is a general optimization algorithm for estimating the parameters of a probabilistic model with latent variables. The algorithm iteratively performs two steps: first, it estimates the expected value of the latent variables using the current parameters, and second, it updates the parameters by maximizing the likelihood function of the model.

[0050] Expected step (E-step): Update the posterior probability of a correct match using Equation 14.

[0051]

[0052] Maximization step (M-step): The objective function to be maximized is shown in Equation 15.

[0053]

[0054] By differentiating the above equation and setting the derivative to zero, we can obtain the following formula for updating the Gaussian distribution parameters.

[0055]

[0056] As can be seen from Equation 4, the update method of the optical flow field coefficients t1 and t2 defined in Equation 2 is as shown in Equation 17.

[0057]

[0058] Where the probability matrix P = diag(p1, p2, ..., p N ).

[0059] After the EM algorithm converges, through p iThe algorithm compares the feature points with a predefined threshold to determine if they are correctly matched. If an incorrect feature point match is detected, the pair of feature points is directly removed. The third step only weights correctly matched feature points, while simultaneously masking all feature points in clusters with support factors below a certain threshold. The second and third steps together enhance the algorithm's robustness against visual feature points. The resulting optical flow field can be further used to determine the disparity between two optimized frames and to select keyframes within the sliding window of the pose estimator.

[0060] Step 3: Adaptively adjust the visual weights of the pose estimator and perform region masking.

[0061] The support factor obtained by the above algorithm can characterize the consistency level of motion of each visual feature point in the optical flow field, thus representing the reliability of feature tracking. Here, the support factor generated in the first step takes values ​​in the range [-1, 1]. Experiments have shown that when the support factor is negative, it can usually be judged as a mismatch and removed by the EM algorithm. Using the median of the remaining support factors as the unit weight benchmark, the weights are scaled proportionally to the visual weights and substituted into the maximum a posteriori estimate of the tightly coupled optimization.

[0062] When a cluster receives less support from other cluster centers than the mean of all support factors, or when more than half of the displacement vectors in a cluster are identified as mismatches, this cluster is considered to have detected an unstructured region. All feature points in this cluster will be masked, and the optimization system will clear all residuals generated by it by assigning a visual weight of 0. In fact, when a displacement vector is identified as a mismatch by the EM algorithm, the corresponding feature point will be removed from the current frame's feature sequence. Even if it is observed in subsequent frames, it will be identified as a new feature, and the temporal correlation effect cannot be restored. In irregular, textureless regions or scenes with moving objects, such as arched doorways of a certain width or naturally swaying flag barriers, single-frame cluster removal can lead to a significant decrease in the number of feature points. Accumulation over several consecutive frames creates feature gaps, severely impacting the operation of tightly coupled systems. Therefore, this approach only optimizes residual masking for mismatched clusters, rather than removing them during the feature tracking stage.

[0063] Step 3: The support factor obtained in Step 2 is used as a weighting coefficient to enhance the visual feature reprojection factor obtained in Step 1. The enhanced visual feature reprojection factor, together with the IMU pre-integration factor and the marginalization factor obtained by Schur marginalization to retain prior information, is used for back-end nonlinear factor graph optimization to estimate and optimize the system's state variables, so that the residual between all observed data and estimated state is minimized, thus realizing monocular visual inertial fusion positioning based on optical flow smoothness constraints.

[0064] Compared with the prior art, the beneficial technical effects of the present invention are as follows:

[0065] This invention provides a monocular visual-inertial fusion localization method based on optical flow smoothness constraints. This method constructs support factors through an optical flow field model to achieve mismatch elimination and adaptive weighting of feature points, thereby optimizing the visual feature point tracking mechanism and the tightly coupled optimization process of visual residuals. This improvement effectively avoids localization drift caused by mismatches of visual feature points during high-frequency sensor data acquisition, while significantly mitigating the adverse effects of image texture loss and moving object interference on system localization performance, thus improving the accuracy and robustness of visual-inertial odometry. Attached Figure Description

[0066] Figure 1 This is a flowchart of a monocular vision-inertial fusion localization method based on optical flow smoothness constraints according to an embodiment of the present invention. Specific implementation methods

[0067] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0068] This invention provides a monocular visual-inertial fusion positioning method based on optical flow smoothness constraints. By designing a visual feature enhancement algorithm based on optical flow smoothness constraints, the method improves the tracking and removal of feature points in the visual-inertial navigation system and tightly couples and optimizes the visual residual part. This prevents feature point tracking drift from being mistakenly retained during high-frequency acquisition of sensor data in the VIO system. At the same time, it reduces the impact of lack of texture in sensor image data and interference from moving objects on the VIO system, thereby achieving monocular visual-inertial fusion positioning.

[0069] In specific implementation of this invention, such as Figure 1 The diagram illustrates the workflow of the visual-inertial localization method, which includes data acquisition and preprocessing, visual feature enhancement based on optical flow smoothness constraints, and local optimization to generate carrier pose prediction results. The specific methods are as follows:

[0070] Step 1: Subscribe to topics published in the ROS system to obtain visual images and IMU data. The IMU part obtains preliminary estimates of the object's position, velocity, and rotation using raw acceleration and angular velocity data, and acquires the IMU pre-integration factor by accumulating the IMU measurement increments between image frames through pre-integration. The visual part first enhances and removes distortion from the images, extracts feature points using Harris detection, tracks feature points between images using sparse optical flow, and reprojects the tracked same feature points from the previous frame to the next frame using the estimated pose transformation between the two frames to obtain the visual feature reprojection factor in factor map optimization. Set the array F... p This is used to store the pixel position information of published feature points in each optimized frame. If the current feature judgment state is published, it will be compared with F. pThe feature points in the model have a corresponding relationship. That is, the point pairs that are successfully tracked between two optimized frames are input into the visual feature enhancement module, and outliers are removed by the EM algorithm.

[0071] Step 2: The visual feature enhancement algorithm based on smooth optical flow field is established between adjacent optimization frames of the sliding window. It uses the relative position, direction, and length of displacement vectors as criteria to calculate the correlation coefficient and support relationship between vectors, and generates clusters based on distribution density. Support factors for each displacement vector are generated through smoothness constraints. These factors have three main functions: first, they serve as prior information for the mismatch elimination EM algorithm; second, they are added to the objective function of the system's least squares optimization as visual weight factors; and third, combined with clustering information, they perform residual masking for unstructured or non-static regions. The specific steps of the visual feature enhancement algorithm based on smooth optical flow field are explained below:

[0072] Step 2.1: The input of this module is the pair of feature points that were successfully tracked in the latest two frames of the sliding window. The difference between the two can be used to obtain the displacement vector of each pair of points. The distance D of each vector to other vectors is calculated, the support relationship S is calculated, and the correlation coefficient matrix R is calculated based on D.

[0073] Feature tracking tracks the same feature point across different frames. Feature matching matches the same feature point across two frames; the matched feature point pair represents the same feature point in the physical world.

[0074] Step 2.2: Calculate the edge correction factor based on the position of each vector on the image.

[0075] Step 2.3: Sum the rows of matrix R to obtain the density value ρ of each vector, and multiply it by its own edge correction factor. Since density value calculations often produce consecutive extrema in a dense vector region, leading to clustering of class centers, a secondary adjustment to the ρ value is made as shown in Formula 10 to ensure a uniform distribution of class centers.

[0076] Step 2.4: Based on density sorting, traverse all displacement vectors from front to back. If the distance between a vector and any class center is less than μ / 2, then classify the vector into the same class as the nearest class center; otherwise, classify it into a new class center. Continue until all vectors have been classified.

[0077] Step 2.5: Calculate the support factor γ for each vector based on the support relationships and clustering.

[0078] Step 2.6: Initialize the EM algorithm parameters, including initializing the correct matching probability matrix P to an identity matrix and the optical flow field parameter vector to an all-zero vector. Then, calculate the binomial distribution correct matching probability ξ using the support factor.

[0079] Step 2.7: Iterate and update P, the optical flow field vectors t1 and t2, and the Gaussian probability model parameters σ in sequence. 2 The process continues until the parameter change becomes minimal, or the maximum number of iterations is reached, at which point convergence is achieved.

[0080] Step 2.8: Select keyframes based on the converged output of the EM algorithm. If the average length of the generated optical flow field vector exceeds a certain threshold, the frame is considered a keyframe. Similarly, if the number of successfully tracked point pairs in Step 2.1 is less than a certain threshold, it is considered that the difference between the two optimized frames is significant, and this frame is also considered a keyframe. This judgment only affects the marginalization operation of the sliding window.

[0081] Step 2.9: Return the feature point numbers with a correct matching probability less than the preset threshold (ξ less than 0.5) to the data preprocessing module for direct removal.

[0082] Step 2.10: Record the number of class vectors as less than a preset threshold (e.g., 5), or the center vector receives less support from other cluster centers than the mean of all support factors, or more than half of the displacement vectors in the class are judged as mismatched classes. These classes are collectively referred to as defective classes. Clear all vector support factors in the defective classes to zero.

[0083] Step 2.11: Finally, the support factor is passed as an attribute of the feature points in the new image to the pose estimator to weight the visual feature reprojection factor. Here, the support factor of the feature points detected during data preprocessing can be arbitrarily assigned, and feature points that are not tracked more than twice consecutively are not included in the reprojection error calculation. Thus, visual feature enhancement is achieved through visual feature preprocessing, namely mismatch removal and weighting.

[0084] Step 3: The enhanced visual feature reprojection factor, IMU pre-integration factor, and marginalization factor are jointly optimized by the back-end nonlinear factor map to achieve monocular visual inertial fusion localization based on optical flow smoothness constraints;

[0085] Back-end nonlinear factor graph optimization primarily estimates and optimizes the system's state variables to minimize the error between all factors (observed data) and nodes (estimated states). Represents the full state vector in the sliding window:

[0086]

[0087] Where, x k This represents the IMU state at the time of capturing the k-th frame. It includes the IMU's position in the world coordinate system. speed and posture And the accelerometer bias b in the IMU coordinate system a and gyroscope bias bg n is the total number of optimized frames, m is the total number of features in the sliding window, and λ l This is the first time the inverse depth of the l-th feature has been observed.

[0088] Minimizing the sum of the prior and Mahalanobis norms of all measurement residuals yields the maximum a posteriori estimate:

[0089] {r p H p To utilize the prior information obtained from Schur supplementation marginalization, we represent the constraints that are removed from the sliding window but retained as historical information while optimizing states are removed. It is the set of all IMU measurements. It is the set of features that are observed at least twice in the current sliding window. The IMU residual is defined as the difference between the estimated relative pose between optimized frames and the IMU measurement. The visual measurement residual is defined, which is generated by the projection of feature points onto the imaging plane. It is the set of all IMU measurements. It is the set of features that are observed at least twice in the current sliding window. The weights of visual feature points obtained through support factors.

[0090] When a new optimization frame is added to the sliding window, the Ceres solver is used to solve the nonlinear problem, and the latest generated pose estimate is saved as the result of the system optimization, where the position estimate of the latest frame is... This represents the real-time localization result of the VIO system. Finally, IMU state and feature marginalization are performed, and the measurements corresponding to the marginal states are converted into priors. When the second most recent frame is a keyframe, the oldest frame in the window and its corresponding measurement are marginalized; if the second most recent frame is a non-keyframe (i.e., it largely overlaps with the previous frame based on visual information), the visual measurement is discarded, and the IMU measurement connected to this non-keyframe is retained as prior information for the next optimization.

[0091] In summary, this invention provides a monocular visual-inertial fusion localization method based on optical flow smoothness constraints. This method constructs support factors through an optical flow field model to achieve mismatch elimination and adaptive weighting of feature points, thereby optimizing the visual feature point tracking mechanism and the tightly coupled optimization process of visual residuals. This improvement effectively avoids localization drift caused by mismatches of visual feature points during high-frequency sensor data acquisition, while significantly mitigating the adverse effects of image texture loss and moving object interference on system localization performance, thus improving the accuracy and robustness of visual-inertial odometry.

[0092] It should be noted that the purpose of disclosing the embodiments is to help further understand the present invention. However, those skilled in the art will understand that various substitutions and modifications are possible without departing from the scope of the present invention and the appended claims. Therefore, the present invention should not be limited to the content disclosed in the embodiments, and the scope of protection of the present invention is defined by the scope of the claims.

Claims

1. A monocular vision inertial fusion positioning method based on optical flow smoothness constraint, characterized in that, By designing a visual feature enhancement algorithm based on optical flow smoothness constraint, the feature point tracking and elimination in visual inertial navigation system is improved, and the visual residual part is tightly coupled and optimized; including the following steps: Step 1, obtain the visual image and IMU data; The preliminary position, velocity and rotation estimation of the object is obtained by using the IMU data, and the IMU pre-integration factor in the factor graph optimization is obtained by accumulating the IMU measurement increments between image frames; the feature points are extracted from the image data and tracked between images, and the visual feature re-projection factor in the factor graph optimization is obtained by estimating the pose transformation between two frames; Step 2, design a visual feature enhancement algorithm based on smooth optical flow field, including: First, generate clustering results and support factors for each image feature point tracking vector in the collected visual image by optical flow smoothness constraint; Second step: construct a complex expectation maximization EM algorithm, use the support factor as prior information in the algorithm solution, eliminate the mis-matching point pairs, and obtain the correct matching vector; Third step: adjust the visual weight of the pose estimator adaptively according to the correct matching vector, and perform region shielding; Step 3, use the support factor obtained in step 2 as a weighted coefficient to enhance the visual feature re-projection factor obtained in step 1, and perform the back-end nonlinear factor graph optimization on the enhanced visual feature re-projection factor and the IMU pre-integration factor to estimate and optimize the state variables of the system, that is, to realize monocular visual inertial fusion positioning based on optical flow smoothness constraint.

2. The monocular vision-inertial fusion positioning method based on the smoothness constraint of optical flow according to claim 1, wherein, In step 1, the same feature point is projected from the previous frame to the next frame by estimating the pose transformation between two frames, and the visual feature re-projection factor in the factor graph optimization is obtained.

3. The monocular vision-inertial fusion positioning method based on the smoothness constraint of optical flow according to claim 1, wherein, In the first step, for the collected visual image, generate clustering results and support factors for each image feature point tracking vector by optical flow smoothness constraint; including the following processes: Step 2.1: first, establish an optical flow field model: recover the optical flow field between two visual images by image feature point tracking vector, estimate the optimal solution of the optical flow field, further constrain the solution of the optical flow field, and obtain the optical flow field coefficient; Input the tracking successful feature point pairs in the latest two frames in the sliding window, and get the displacement vector of each point pair by subtracting the two, calculate the distance D of each vector and other vectors, support relationship S, and calculate the correlation coefficient matrix R according to D; Step 2.2: calculate the edge correction factor θ according to the position of each vector on the image; Step 2.3: sum the matrix R by row to obtain the density value ρ of each vector, and multiply it by the edge correction factor; at the same time, adjust the ρ value twice to make the class centers evenly distributed; Step 2.4: sort all displacement vectors according to the density, if the distance between the vector and any class center is less than the set threshold, then the vector and the class center with the closest distance are divided into the same class, otherwise a new class center is divided, until the classification of all vectors is completed; Step 2.5: calculate the support factor γ of each vector according to the support relationship and clustering.

4. The monocular vision-inertial fusion positioning method based on the smoothness constraint of optical flow according to claim 3, wherein, The solution method of optical flow field is represented as: Wherein, M1, M2 represents the x-axis component and y-axis component of each displacement vector in the set M, forming an N x 1 column vector; M represents the set of all tracking feature point displacement vectors; t1 and t2 are N x 1 column vectors composed of real number series, representing the contribution weight of the feature point to the two-dimensional component of the optical flow field; R is the kernel function, an N x N matrix, representing the correlation degree between each two features.

5. The monocular vision-inertial fusion positioning method based on the smoothness constraint of optical flow according to claim 4, wherein, The optical flow field coefficients are obtained by solving the linear system represented as follows: Wherein, λ is a regularization parameter, used to balance the error and the complexity of the solution, and I is the unit matrix.

6. The monocular vision-inertial fusion positioning method based on the smoothness constraint of optical flow according to claim 5, wherein, In step 2.4, the vectors are classified, specifically according to the density order, all displacement vectors are traversed from the front to the back; the threshold is set to μ / 2, μ represents the mean distance between vectors, that is, the mean of the D matrix.

7. The monocular vision-inertial fusion positioning method based on smoothness constraint of optical flow according to claim 5, wherein, The second step is to construct a complex expectation maximization (EM) algorithm, including the following processes: Step 2.6: Initialize the EM algorithm parameters, including initializing the probability matrix P of correct matching to the unit matrix, and initializing the parameter vector of the optical flow field to the all-zero vector; and calculating the probability ξ of feature point matching using the support factor; Step 2.7: Update the likelihood function P, the optical flow field vectors t1, t2, and the Gaussian probability model parameters σ in turn in a loop 2 until the loop terminates, and convergence is completed; Step 2.8: Select the key frame according to the output result of the EM algorithm convergence; specifically, the frame whose generated optical flow field average vector length exceeds the set threshold or the number of successfully tracked point pairs is less than the threshold is the key frame; Step 2.9: Remove the feature points with a correct matching probability less than a preset threshold.

8. The monocular vision-inertial fusion positioning method based on smoothness constraint of optical flow according to claim 7, wherein, The third step is to adaptively adjust the visual weight of the pose estimator and perform regional shielding, specifically: Step 2.10: If the number of class vectors is less than a preset threshold, or the support of the center vector by other cluster centers is less than the mean of all support factors, or more than half of the displacement vectors in the class are identified as mis-matching, then set the support factor of all vectors in the mis-matching class to zero; Step 2.11: Use the support factor as an attribute of the feature points in the new image to input into the pose estimator, and weight the visual feature re-projection factor; thus, by preprocessing the visual features, i.e., mis-matching removal and weighting, the visual feature enhancement is realized.

9. The monocular vision-inertial fusion positioning method based on smoothness constraint of optical flow according to claim 8, wherein, The value range of the support factor generated in the first step is set to [-1, 1]; in the third step, when the support factor is negative, the EM algorithm judges it as mis-matching and removes it.

10. The monocular vision-inertial fusion positioning method based on smoothness constraint of optical flow according to claim 9, wherein, In step 3, the enhanced visual feature re-projection factor is combined with the IMU pre-integration factor and the marginalized factor obtained by marginalizing the prior information through Schur complement to perform the back-end nonlinear factor graph optimization.

Images