Visual inertia fusion positioning method and system based on feature optimization

Abstract

The invention relates to a visual inertial fusion positioning method and system based on feature optimization, and belongs to the technical field of visual inertial navigation of autonomous robots. According to the method and the system, on the basis of a Geometric Feature Track Selection Visual Native Navigation System (GFT-VINS), through a feature trajectory selection strategy based on normal epipolar geometry, high-quality feature trajectories are screened out, low-quality trajectory interference is avoided, the positioning precision is improved, error accumulation caused by environmental changes and feature anomalies is reduced, and the system stability is enhanced. Meanwhile, in combination with a multi-state Constraint Kalman filter (MSCKF) framework, the efficient calculation capability is kept, the requirements of the autonomous robot for high precision, high stability and efficient calculation in a complex environment are met, and wide application of the autonomous robot is promoted.

Description

Technical Field

[0001] The present invention belongs to the technical field of visual inertial navigation of autonomous robots, and relates to a visual inertial fusion positioning method and system based on feature optimization. Background Art

[0002] In the field of visual inertial navigation systems (VINS), the existing technologies mainly focus on optimization-based and filtering-based methods. These technologies provide an important foundation for subsequent research, but they also have significant defects.

[0003] Visual positioning determines the pose of a camera by using images by capturing the correspondence of visual features or optical flow between adjacent frames, and is crucial in many application tasks such as Augmented Reality (AR), autonomous vehicles, and robot navigation. Although visual positioning is a commonly used positioning method in an environment where Global Navigation Satellite Systems (GNSS) signals are limited, it is vulnerable to illumination conditions, moving objects, and textureless scenes.

[0004] To solve the above problems, visual inertial navigation systems (Visual Inertial Navigation Systems, VINS) have been developed in recent years. VINS cleverly integrates the measurement data of on-vehicle cameras and inertial sensors, and uses the complementary characteristics of the two to achieve robust and accurate state estimation. For decades, a large number of studies have been dedicated to developing accurate, robust, and efficient visual inertial fusion systems, resulting in numerous advanced methods, mainly divided into optimization-based methods and filtering-based methods.

[0005] Optimization-based methods, such as VINS-Fusion, ORB-SLAM3, OKVIS, etc., use the feature trajectories and inertial data at multiple moments to solve the non-linear least squares problem through iterative batch optimization. Although this method can achieve high state estimation accuracy, the computational cost is high. For example, VINS-Fusion relies on the Ceres optimization framework and is inefficient in processing high-dimensional state variables and observations, with an average processing time of up to 33 ms per frame, and is not very suitable for scenarios with high real-time requirements.

[0006] Filter-based methods mostly rely on the Extended Kalman Filter (EKF), represented by OpenVINS. These methods use the feature trajectories of the current frame or a small number of recent frames for recursive state estimation, with relatively low computational complexity and high computational efficiency. However, in real complex environments, filter-based methods also face many challenges. Due to factors such as environmental structure and sensor limitations, features may aggregate or be unevenly distributed, resulting in unbalanced geometric constraints in different degrees of freedom and affecting the positioning accuracy. Light changes, dynamic objects, and sensor noise will generate outliers, which will introduce biases during the recursive update process, leading to continuous accumulation of estimation errors. When encountering situations such as poor lighting, textureless surfaces, or occlusions, the information contained in the feature trajectories may be insufficient to provide sufficient constraints for state estimation, thus seriously affecting the accuracy of positioning and the robustness of the system.

[0007] Existing technologies are difficult to achieve a balance in terms of the accuracy, computational efficiency, and robustness of visual inertial navigation systems, and cannot meet the requirements of complex and changing practical applications, thus urgently needing improvement. Summary of the Invention

[0008] In view of this, the purpose of the present invention is to provide a visual inertial fusion positioning method and system based on feature optimization. This method and system are based on the Geometric Feature Track Selection Visual Inertial Navigation System (GFT-VINS). Through a feature trajectory selection strategy based on normal epipolar geometry, high-quality feature trajectories are screened out, avoiding interference from low-quality trajectories, improving the positioning accuracy, reducing the error accumulation caused by environmental changes and feature anomalies, and enhancing the system stability. At the same time, combined with the Multi-State Constraint Kalman Filter (MSCKF) framework, it maintains high computational efficiency and meets the requirements of autonomous robots for high precision, high stability, and high computational efficiency in complex environments, promoting its wide application.

[0009] To achieve the above purpose, the present invention provides the following technical solutions:

[0010] A visual inertial fusion positioning method based on feature optimization, which specifically includes the following steps:

[0011] S1: Build a fusion framework: Build a visual inertial fusion framework based on the Multi-State Constraint Kalman Filter (MSCKF) for fusing visual data and Inertial Measurement Unit (IMU) data;

[0012] S2: Measurement preprocessing: including: using the sparse optical flow tracking algorithm combined with inertial measurement unit pre-integration to establish 2D observation correspondences, and using IMU pre-integration to achieve trajectory prediction while reducing computational complexity by discretely integrating continuous IMU measurements; calculating the motion vectors of 2D points using the SE2 Lie parameterization method, and adopting an improved optical flow tracking method, using the Locally-scaled Sum of Squared Differences (LSSD) method to mitigate the impact of illumination changes on tracking, introducing the Lie algebra manifold as the motion model to more accurately describe the complex motion of feature points, thereby improving the tracking accuracy;

[0013] S3: Feature trajectory selection, including:

[0014] S31: Screening feature points with high visual saliency based on the Shi-Tomasi score to generate a candidate feature subset;

[0015] S32: Using the normal epipolar geometry constraint to construct an attitude optimization problem, and removing geometrically inconsistent outliers through the RANSAC method;

[0016] S33: Dividing the candidate feature subset into multiple partitions, and selecting evenly distributed and information-rich feature trajectories from each partition through a submodular optimization algorithm that maximizes the log-determinant value;

[0017] S4: State estimation: Propagating, augmenting, and updating the state through the MSCKF framework, constructing multi-frame visual constraints using the selected feature trajectories, and minimizing the reprojection error to achieve accurate attitude estimation.

[0018] Furthermore, in step S2, the IMU collects data at a high frequency. Assuming there are k IMU measurements between two consecutive visual frames i and j, the specific IMU pre-integration method includes: calculating the relative rotation ΔR k and velocity Δv k , and translation Δp ij between the two frames by discretely integrating the angular velocity ω ij and acceleration a ij of the IMU, and the formula is:

[0019]

[0020] where and are the biases of the gyroscope and accelerometer respectively.

[0021] Furthermore, in step S2, the optical flow tracking method includes:

[0022] Initialize the corresponding relationship of feature points using the relative pose [R, t] obtained by IMU pre-integration. Let p and p′ be the two-dimensional feature points of the same landmark in two consecutive frames I t-1 and I t respectively. According to the following formula:

[0023] dK -1 p = d′RK -1 p′ + t

[0024] Under certain conditions, it is simplified to the formula:

[0025] p = KRK -1 p′

[0026] Obtain the predicted position of the feature point in the current frame, where K is the camera internal parameter matrix and R is the rotation matrix obtained by IMU pre-integration.

[0027] Furthermore, in step S2, when calculating the optical flow, the LSSD method is used to calculate the photometric error:

[0028]

[0029] Update the motion parameters of the feature points by minimizing this error; at the same time, introduce the Lie algebra manifold as the motion model, and through the formula:

[0030]

[0031] Perform multi-scale tracking, gradually calculate the optical flow tracking error from coarse to fine, and obtain the feature correspondence set U.

[0032] Furthermore, in step S3, in the partition selection step, select the feature trajectory through the following optimization formula:

[0033] arg max logdet(M(S)), subject to |S ∩ P i | = 1

[0034] Adopt the random partition greedy algorithm. In each iteration, randomly select a partition from U”, and select the feature pair that maximizes the value of the logarithmic determinant in this partition to obtain an information-rich and evenly distributed feature trajectory set S.

[0035]

[0036] Furthermore, in step S4, the state update step includes:

[0037] Calculate the three-dimensional landmark point through triangulation and construct multi-frame visual constraints;

[0038] Project the visual observation model into the null space and simplify the residual model through QR decomposition;

[0039] Update the system state and covariance matrix, with the formula:

[0040]

[0041] Δx = Kr n

[0042] P k+1|k+1 =(I - KT H )P k+1|k (I - KT H ) T + KR n K T 。

[0043] Furthermore, the system includes: a fusion framework module, a preprocessing module, a feature selection module, and a state estimation module;

[0044] The fusion framework module is used to fuse visual and inertial data based on the MSCKF framework; the preprocessing module is configured to perform IMU pre-integration and optical flow tracking and output the predicted trajectory of feature points; the feature selection module includes an appearance filtering unit, a normal epipolar verification unit, and a partition optimization unit for screening high-quality feature trajectories; the state estimation module realizes state propagation, expansion, and update through the MSCKF algorithm and outputs accurate positioning results.

[0045] Furthermore, in the feature selection module, the normal epipolar verification unit constructs an attitude optimization problem through the following formula:

[0046] R * ,t * = argmint T M(R)t

[0047] where M(R) = ∑(p i × Rp′ i )(p i × Rp′ i ) T , where p i and p i ′ are the matching feature points between two frames, and R and t are the rotation matrix and translation vector.

[0048] The beneficial effects of the present invention are as follows:

[0049] 1) High precision positioning: Tested on a desktop computer equipped with an i5-8400 CPU and 16GB RAM using the EuRoC dataset. Taking the RMSE of the translational component in ATE as the index, the detailed comparison results are shown in Table 1. The average positioning error of GFT-VINS is 0.062m, and the accuracy is at least 26.1% higher than other methods. It can be seen that selecting feature trajectories based on normal epipolar geometry can significantly reduce observation errors, provide reliable geometric constraints, and achieve precise positioning.

[0050] 2) High computational efficiency: The feature trajectory selection method used by GFT-VINS can quickly extract a small number of reliable visual constraints, significantly reducing the computational complexity of the backend. At the same time, combined with the MSCKF framework for visual-inertial fusion, compared with the method based on nonlinear optimization, it has higher computational efficiency, ensuring that the robot can respond to environmental changes in a timely manner. According to Figure 5 the results in, its average processing time is at least 8ms, and the processing speed reaches 125FPS. While the average processing time of VINS-Fusion is 33ms / frame, and that of ICE-BA is 16ms / frame.

[0051] 3) Strong anti-interference ability: GFT-VINS filters the observation data with large deviations and outliers. By selecting more informative feature trajectories, it enhances the stability of attitude constraints, thus ensuring accurate estimation at all times. According to the comparison experiment results, calculate the mean and standard deviation based on the root mean square error (RMSE) of the benchmark method on different sequences, and use this as an index of system robustness. The comparison results are as Figure 6 shown. GFT-VINS maintains highly stable performance on all sequences, with the lowest mean of 0.062 meters and the lowest standard deviation of 0.03 meters. Compared with other benchmark methods, these indicators are at least 26.1% and 43.3% higher respectively.

[0052] Other advantages, objectives, and features of the present invention will be described to some extent in the subsequent specification, and to some extent, will be obvious to those skilled in the art based on the study of the following text, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be achieved and obtained through the following specification. Brief Description of the Drawings

[0053] In order to make the objectives, technical solutions, and advantages of the present invention clearer, the present invention will be described in detail and preferably with reference to the accompanying drawings, where:

[0054] Figure 1 It is the architecture diagram of GFT-VINS;

[0055] Figure 2It is a comparison chart of the overall performance between GFT-VINS and existing methods;

[0056] Figure 3 It is a schematic diagram of feature trajectory selection processing;

[0057] Figure 4 It is a result chart of the attitude estimation simulation experiment;

[0058] Figure 5 It is a comparison chart of the average processing time per frame (ms) of each baseline method;

[0059] Figure 6 It is a comparison chart of RMSE standard deviation;

[0060] Figure 7 It is a result chart of the ablation experiment on feature trajectory selection;

[0061] Figure 8 It is a result chart of the ablation experiment on each evaluation criterion;

[0062] Figure 9 It is a cumulative distribution chart of position errors under different experimental configurations;

[0063] Figure 10 It is a visualization comparison chart of feature trajectory selection. Detailed implementation manner

[0064] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0065] In the field of autonomous robots, VINS is crucial for the positioning of robots during autonomous navigation tasks. However, existing VINS still has inevitable defects. Optimization-based methods have high computational costs and are difficult to process data in real time, resulting in positioning delays; filtering-based methods rely heavily on the quality of feature trajectories. In complex environments, feature trajectories are prone to problems such as uneven distribution, noise, and outliers, leading to large positioning errors and poor stability. The present invention provides a visual-inertial fusion positioning method and system based on feature optimization, which relates to the technical field of visual-inertial navigation of autonomous robots, and particularly to the technology of precise positioning through the fusion of visual and inertial sensors. Figure 1 It is a architecture diagram of GFT-VINS, Figure 1 clearly showing three main components: measurement preprocessing, feature trajectory selection, and state estimation, as well as their data flow directions, helping to understand the overall process of the invention and the relationships between various parts. Figure 2 It is a comparison chart of the overall performance between GFT-VINS and existing methods, Figure 2In the figure, the left figure presents the results of each step of the feature trajectory selection process, including screening out visually significant, evenly distributed, and information-rich feature trajectories; the middle figure compares the trajectory of GFT-VINS with the ground truth trajectory on the V202 sequence of the EuRoC dataset; the right figure evaluates GFT-VINS and existing methods in terms of positioning accuracy, error robustness, and computational efficiency, intuitively reflecting the advantages of the present invention.

[0066] In this embodiment, taking the indoor navigation of an unmanned aerial vehicle based on GFT-VINS as an example, the implementation process of the technical solution of the present invention is described in detail:

[0067] 1. Measurement preprocessing:

[0068] IMU pre-integration: The IMU carried by the unmanned aerial vehicle collects data at a high frequency. Assume that there are k IMU measurements between two consecutive visual frames i and j. According to formula (1):

[0069]

[0070] Calculate the pre-integrated rotation ΔR ij and velocity Δv ij and translation Δp ij . For example, during a certain flight time, by integrating the angular velocity ω k , acceleration a k and bias of the IMU measurements, the relative motion information between two frames is obtained for the initialization of subsequent optical flow tracking.

[0071] Optical flow tracking: Use the relative pose [R, t] obtained by IMU pre-integration to initialize the correspondence of feature points. Let p and p′ be the two-dimensional feature points of the same landmark in two consecutive frames I t-1 and I t respectively. According to formula (2)

[0072] dK -1 p = d′RK -1 p′ + t #(2) is simplified to formula (3) under certain conditions:

[0073] p = KRK -1 p′ #(3)

[0074] Obtain the predicted position of the feature point in the current frame. For example, given the camera internal parameter matrix K and the rotation matrix R obtained by IMU pre-integration, the position of the feature point in the new frame can be calculated to improve the tracking accuracy.

[0075] When calculating the optical flow, the LSSD method is used to calculate the photometric error, formula (4):

[0076]

[0077] Update the feature point motion parameters by minimizing this error. Meanwhile, introduce the Lie algebra manifold as the motion model, through Equation (5):

[0078]

[0079] Perform multi-scale tracking, gradually calculate the optical flow tracking error from coarse to fine, and obtain the feature correspondence set U.

[0080] 2. Feature trajectory selection:

[0081] Appearance filtering: For the images captured by the UAV, calculate the Shi-Tomasi score for each feature point, which is the minimum eigenvalue in the correlation matrix G of Equation (6).

[0082]

[0083] Select 150 matching pairs with higher scores to form a subset U′. These feature points have significant appearances and are beneficial for subsequent tracking.

[0084] Normal epipolar line verification: For the corresponding features p i and p′ i between two consecutive frames, according to Equation (7):

[0085] n i = p i × Rp′ i #(7)

[0086] Calculate the normal vector of the epipolar plane, and this normal vector should satisfy t T n i = 0. Based on this, construct the optimization problem regarding the pose parameters [R, t] as Equation (8):

[0087] R * , t * = argmin t T M(R)t #(8)

[0088] where M(R) = ∑(p i × Rp′ i )(p i × Rp′ i ) T .

[0089] Through the RANSAC method, randomly select five pairs of feature trajectories to calculate the pose parameters, verify the remaining point pairs, remove the outliers, and obtain a set of point pairs U” that satisfy geometric consistency.

[0090] Partition Selection: The candidate feature trajectory set U” is divided into k non - overlapping partitions P according to the feature positions i , satisfying

[0091] Under the constraint |S∩P i | = 1, the feature trajectory selection problem is transformed into a sub - modular partition optimization problem, as shown in Equation (9):

[0092] arg max logdet(M(S)), subject to |S∩P i | = 1#(9)

[0093] Adopt a random partition greedy algorithm. In each iteration, randomly select a partition from U”, and within this partition, select the feature pair that maximizes the log - determinant value in Equation (10) to obtain an information - rich and evenly - distributed feature trajectory set S.

[0094]

[0095] Figure 3 Fig. [X] is a schematic diagram of the feature trajectory selection process. Among them, (a) shows the result after optical flow tracking, and (b - d) respectively show the tracking results after appearance filtering, normal epipolar verification, and partition selection. (e) and (f) illustrate the translational and rotational estimation errors at different processing stages. Starting from the original feature trajectory, the tracking results after appearance filtering, normal epipolar verification, and partition selection are successively shown. At the same time, through the translational and rotational error maps of pose estimation, the improvement effect of each processing step on the pose estimation accuracy is intuitively reflected.

[0096] Figure 4 Fig. [X] is a graph of the results of the pose estimation simulation experiment. Among them, (a) shows a three - dimensional simulation environment. At a 60 - degree field - of - view angle, randomly generated landmarks are distributed in the range of depth [10, 50]. The standard deviation of depth noise is 1%, and the standard deviation of angular noise is 1 degree. (c) and (e) are the observations of the landmarks from two different viewpoints. By randomly selecting different numbers of feature trajectories, we estimate the poses of the two viewpoints according to their correspondences and calculate the translational error (b) and rotational error (d) relative to the true values. In addition, the relationship between the number of points used and the spectral property metric value is recorded (f).

[0097] As can be seen from Table 1, generally speaking, the proposed GFT - VINS system shows the best accuracy. Compared with other baseline methods, this method achieves the lowest RMSE in more than half of the sequences and can also reach an accuracy similar to that of the best - performing method in the remaining sequences. The average positioning error reaches 0.062 meters, which is at least 26.1% higher than other benchmark methods.

[0098] Table 1 Comparison of Root Mean Square Error of Absolute Trajectory Error on EuRoC Dataset

[0099]

[0100] 3. State Estimation:

[0101] State Propagation: According to the IMU measurement data, based on formula (11):

[0102]

[0103] Calculate the covariance matrix, where

[0104]

[0105] Realize the real-time propagation of the system state.

[0106] State Augmentation: When there is a new camera frame, calculate the pose of the new camera frame in the global coordinate system according to formula (12):

[0107]

[0108] Add it to the system state vector and augment the covariance matrix according to formula (13):

[0109]

[0110] Where the calculation formula of matrix J:

[0111]

[0112] State Update: Utilize the selected high-quality feature trajectories, calculate the 3D landmark points through triangulation, construct multi-frame visual constraints, and project the visual observation model formula (14):

[0113]

[0114] Project onto The null space of is simplified to formula (15):

[0115]

[0116] Based on this, construct and combine the residual models of all 3D landmark points. After QR decomposition, formula (16) is obtained:

[0117]

[0118] Finally, according to formulas (17)-(19):

[0119]

[0120] Δx = Krn #(18)

[0121] P k+1|k+1 = (I - KT H )P k+1|k (I - KT H ) T + KR n K T #(19)

[0122] Update the system state and covariance matrix to achieve precise navigation of the UAV indoors.

[0123] Figure 5 This is the comparison chart of the average processing time per frame (ms) for each baseline method. Figure 6 This is the comparison chart of the RMSE standard deviation. It can be seen from Figure 6 that GFT-VINS maintains a highly stable performance in all sequences, with the lowest mean value of 0.062 meters and the lowest standard deviation of 0.03 meters. Compared with other baseline methods, these metrics are improved by at least 26.1% and 43.3% respectively.

[0124] Figure 7 This is the result chart of the ablation experiment for feature trajectory selection, comparing the root mean square error of the absolute trajectory error (RMSEATE, left figure) and the average processing time per frame (right figure) of four configurations: the one with feature tracking selection enabled (GFT-VINS), the one without feature tracking selection enabled (GFT-VINS-wo), and the configurations of OpenVINS using up to 25 pairs of matching points (OpenVINS-25) and up to 50 pairs of matching points (OpenVINS-50) respectively.

[0125] Figure 8 This is the result chart of the ablation experiment for each evaluation criterion, which is an ablation study of each evaluation criterion on the EuRoC dataset. On the EuRoC dataset, all experimental configurations are compared in terms of the appearance saliency of the feature trajectory measured by the Shi-Tomasi score (left figure), the outlier ratio (middle figure), and the log determinant value (right figure).

[0126] Figure 9 This is the cumulative distribution chart of the position error under different experimental configurations. Comparative experiments were conducted on the MH01 - MH04 sequences (a - c). In these curves, each point represents the proportion (y-axis) of instances where the position error is lower than a given threshold (x-axis). GFT-VINS with feature tracking selection enabled is located in the upper left corner, which highlights the effectiveness of the feature selection method in improving the positioning accuracy. Figure 10The visualization comparison graph selected for the feature trajectory shows that the method proposed by the present invention realizes a more uniform distribution of the selected features in the image space, while avoiding the situation of excessive aggregation in the high-reflection areas, because these areas are prone to introducing potential outliers.

[0127] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified without departing from the purpose and scope of the present technical solution, and they should all be covered within the scope of the claims of the present invention.

Claims

1. A visual-inertial fusion localization method based on feature optimization, characterized in that: The method specifically includes the following steps: S1: Build a fusion framework: Construct a visual-inertial fusion framework based on the Multi-State Constraint Kalman Filter (MSCKF) for fusing visual data and Inertial Measurement Unit (IMU) data; S2: Measurement preprocessing: Include: Use the sparse optical flow tracking algorithm combined with IMU pre-integration to establish 2D observation correspondences, and use IMU pre-integration to discretely integrate continuous IMU measurements to achieve trajectory prediction while reducing computational complexity; Use the SE2 Lie parameterization method to calculate the motion vector of 2D points, and adopt an improved optical flow tracking method. Use the Locally-scaled Sum of Squared Differences (LSSD) method to reduce the impact of illumination changes on tracking, and introduce the Lie algebra manifold as a motion model to describe the complex motion of feature points; S3: Feature trajectory selection, including: S31: Screen feature points with high visual saliency based on the Shi-Tomasi score to generate a candidate feature subset; S32: Use the normal epipolar geometry constraint to construct an attitude optimization problem, and remove geometrically inconsistent outliers through the RANSAC method; S33: Divide the candidate feature subset into multiple partitions, and select evenly distributed and information-rich feature trajectories from each partition through a submodular optimization algorithm that maximizes the log determinant value; S4: State estimation: Perform state propagation, state augmentation, and state update through the MSCKF framework, and use the selected feature trajectories to construct multi-frame visual constraints to minimize the reprojection error to achieve accurate attitude estimation.

2. The visual-inertial fusion positioning method based on feature optimization according to claim 1, wherein: In step S2, the IMU collects data at a high frequency. Assuming there are k IMU measurements between two consecutive visual frames i and j, the specific IMU pre-integration method includes: calculating the relative rotation ΔR k between two frames, the relative velocity Δv k , and the relative translation Δp ij by discretely integrating the angular velocity ω ij and the acceleration a ij of the IMU. The formulas are as follows: wherein, and are the biases of the gyroscope and the accelerometer, respectively.

3. A visual-inertial fusion positioning method based on feature optimization according to claim 2, characterized in that: In step S2, the optical flow tracking method includes: Initialize the correspondence of feature points using the relative pose [R, t] obtained by IMU pre-integration. Let p and p′ be the two-dimensional feature points of the same landmark in two consecutive frames I t-1 and I t respectively. According to the following formula: dK -1 p = d′RK -1 p′ + t Simplify to the formula under certain conditions: p = KRK -1 p' Obtain the predicted position of the feature point in the current frame, where K is the camera intrinsic matrix and R is the rotation matrix obtained by IMU pre-integration.

4. A visual-inertial fusion positioning method based on feature optimization according to claim 3, characterized in that: In step S2, when calculating the optical flow, use the LSSD method to calculate the photometric error: Update the feature point motion parameters by minimizing this error; at the same time, introduce the Lie algebra manifold as a motion model, through the formula: Perform multi-scale tracking, gradually calculate the optical flow tracking error from coarse to fine, and obtain the feature correspondence set U.

5. A visual-inertial fusion positioning method based on feature optimization according to claim 4, characterized in that: In step S3, in the partition selection step, select the feature trajectory through the following optimization formula: arg max logdet(M(S)), subject to |S ∩ P i | = 1 Adopt a random partition greedy algorithm. In each iteration, randomly select a partition from U", and select the feature pair that maximizes the log determinant value in this partition to obtain an information-rich and evenly distributed feature trajectory set S.

6. The visual-inertial fusion positioning method based on feature optimization according to claim 5, characterized in that: In step S4, the state update step includes: Calculate the 3D landmark points through triangulation to construct multi-frame visual constraints; Project the visual observation model into the null space and simplify the residual model through QR decomposition; Update the system state and covariance matrix, and the formula is: Δx = Kr n P k+1|k+1 = (I - KT H )P k+1|k (I - KT H ) T + KR n K T 。 7. A visual-inertial fusion positioning system based on feature optimization, characterized in that: The system includes: a fusion framework module, a preprocessing module, a feature selection module, and a state estimation module; The fusion framework module is used to fuse visual and inertial data based on the MSCKF framework; the preprocessing module is configured to perform IMU pre-integration and optical flow tracking, and output the predicted trajectory of feature points; the feature selection module includes an appearance filtering unit, a normal epipolar verification unit, and a partition optimization unit, and is used to screen high-quality feature trajectories; the state estimation module realizes state propagation, expansion, and update through the MSCKF algorithm, and outputs accurate positioning results.

8. The visual-inertial fusion positioning system based on feature optimization according to claim 7, characterized in that: In the feature selection module, the normal epipolar verification unit constructs an attitude optimization problem through the following formula: R * ,t * = arg min t T M(R)t where M(R) = ∑(p i × Rp′ i )(p i × Rp′ i ) T , where p i and p i ′ are matching feature points between two frames, and R and t are the rotation matrix and the translation vector.

Images