Robust visual inertial navigation system and method based on dynamic feature point filtering

Abstract

The invention discloses a robust visual inertial navigation system and method based on dynamic feature point filtering, and mainly solves the technical problems of low navigation precision caused by dynamic feature point interference and high calculation overhead and poor real-time performance of an existing dynamic feature processing scheme in the prior art. According to the scheme, the system comprises an image acquisition module, an IMU data acquisition module, a VINS front-end module, a rear-end optimization module and a dynamic feature point filtering module additionally arranged at the front end of the VINS, the module adopts a two-stage filtering mechanism combining geometric constraint preliminary screening and lightweight semantic segmentation fine screening, and a parallel-synchronous integration mode is adopted, so that the VINS front-end module and the VINS rear-end optimization module are integrated. Image data only containing the static feature points are sent to the front end of the VINS for subsequent pose calculation, and navigation is achieved. According to the invention, the dynamic feature points in the image can be effectively identified and eliminated, and the calculation overhead is significantly reduced; the method can be used for high-precision positioning navigation of unmanned aerial vehicles, autonomous vehicles and mobile robots in a dynamic environment.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of navigation positioning, further relates to an improved method of a visual-inertial navigation system (VINS), in particular to a robust visual-inertial navigation system based on dynamic feature point filtering, which can be used in automatic driving, unmanned aerial vehicle flight control, robot autonomous navigation, indoor positioning and other scenes with high requirements for navigation accuracy and real-time performance. BACKGROUND

[0002] The visual-inertial navigation system VINS can provide continuous and stable pose estimation for a carrier in an environment without external positioning signals (such as GPS) by fusing visual information collected by a camera and inertial information collected by an inertial measurement unit (IMU), and is one of the core technologies for autonomous mobile devices to realize positioning and navigation. Among them, VINS-Mono, as a typical monocular visual-inertial navigation system, has been widely used in many fields due to its low hardware cost and wide application scenarios. The working process of VINS-Mono mainly includes two parts: front-end and back-end. The front-end is responsible for extracting feature points from continuous image frames, performing feature matching, and pre-integrating with IMU data to provide initial motion estimation and feature association information for the back-end; the back-end constructs a globally consistent pose graph based on the information output by the front-end through a nonlinear optimization method to realize high-precision pose estimation. However, in a dynamic environment (such as a scene with walking pedestrians, driving vehicles, and moving objects), the traditional VINS-Mono has obvious performance defects.

[0003] When extracting feature points, the front-end of the traditional VINS-Mono treats all high-contrast regions in the image as effective feature points for processing, including both static feature points on static scenes (such as walls, floors, and fixed obstacles) and dynamic feature points on dynamic objects (such as pedestrians, vehicles, and moving shelves). Since dynamic feature points produce displacement as objects move, their motion trajectories do not match the true motion trajectories of the carrier, which will introduce significant errors in the feature matching and pose estimation processes. For example, in an autonomous driving scenario, a vehicle driving in front will form dynamic feature points in the camera image. If the traditional VINS-Mono front-end uses these dynamic feature points together with static feature points for pose calculation, it will cause the position estimation of the carrier (vehicle) to deviate. Since VINS analyzes the "motion" of feature points in the image to infer the "motion" of the camera (unmanned aerial vehicle) itself, VINS must deal with static feature points, so a dynamic feature point filtering module is necessary.

[0004] To address the robustness of navigation in dynamic environments, existing technologies have developed several dynamic feature point processing solutions, which can be broadly categorized into two types: one type is dynamic feature recognition methods based on complex geometric constraints, such as using stereo matching of multiple frames of images or trifocal tensor constraints to determine whether feature points are dynamic. However, these methods have high computational complexity, require redundant information from multiple frames of images, and are difficult to meet real-time requirements. The other type is dynamic feature segmentation methods based on deep learning, such as using high-precision semantic segmentation networks (e.g., Mask R-CNN, DeepLab) to semantically label dynamic objects in images, thereby removing dynamic feature points. However, these deep learning models have a large number of parameters and long inference time, making them unsuitable for scenarios with stringent real-time requirements, such as autonomous driving and drones.

[0005] For example, an existing VINS improvement scheme based on semantic segmentation uses ResNet as the backbone network to build the semantic segmentation model. Although it can accurately identify dynamic objects, the inference frame rate of this model on conventional embedded hardware (such as NVIDIA Jetson TX2) is only 8-10fps, far lower than the 20-30fps image processing frame rate of the VINS-Mono front-end. This leads to an increase in overall system latency and cannot meet the real-time closed-loop requirements of "perception-decision-control" in UAVs. In addition, this scheme does not consider the collaborative working mechanism between the semantic segmentation model and the VINS front-end, and there is data redundancy in the connection between the segmentation results and the feature point extraction process, further reducing system efficiency.

[0006] The core requirement of visual inertial navigation systems (VINS) is to provide high-precision and robust pose estimation while ensuring real-time performance. Therefore, designing a low-computational-overhead dynamic feature point processing mechanism that can effectively identify and eliminate dynamic feature points without affecting the system's real-time performance is key to solving the robustness problem of VINS in dynamic environments. Summary of the Invention

[0007] The purpose of this invention is to address the shortcomings of the prior art by proposing a robust visual inertial navigation system and method based on dynamic feature point filtering. This invention aims to solve the technical problems of low navigation accuracy caused by dynamic feature point interference in dynamic environments in existing VINS-Mono systems, as well as the high computational overhead and poor real-time performance of existing dynamic feature processing schemes.

[0008] To achieve the above objectives, the technical solution of the present invention includes the following:

[0009] A robust visual inertial navigation system based on dynamic feature point filtering includes an image acquisition module, an IMU data acquisition module, a dynamic feature point filtering module, a VINS front-end module, and a VINS back-end optimization module; wherein the image acquisition module, the dynamic feature point filtering module, the VINS front-end module, and the VINS back-end optimization module are connected sequentially, and the output of the IMU data acquisition module is connected to the input of the VINS front-end module;

[0010] The image acquisition module is used to acquire image data and output it to the dynamic feature point filtering module;

[0011] The IMU data acquisition module is used to acquire the inertial data of the carrier and output it to the VINS front-end module.

[0012] The dynamic feature point filtering module identifies and removes dynamic feature points in the image by performing a two-stage filtering mechanism on the image data, and outputs image data containing only static feature points to the VINS front-end module.

[0013] The VINS front-end module is used to process the inertial data output by the IMU data acquisition module and the image data output by the dynamic feature point filtering module to obtain the pre-integration result and initial pose information, and output them to the VINS back-end optimization module.

[0014] The VINS back-end optimization module obtains the carrier pose through nonlinear optimization based on the output of the VINS front-end module.

[0015] Furthermore, the aforementioned image acquisition module employs a monocular camera to acquire continuous image frames during the carrier's motion at a preset frame rate as image data; the IMU data acquisition module uses an IMU device including an accelerometer and a gyroscope to acquire the carrier's inertial data at a frequency more than 10 times higher than the image frame rate, including acceleration data acquired by the accelerometer and angular velocity data acquired by the gyroscope; the dynamic feature point filtering module identifies and removes dynamic feature points in the image through a two-stage filtering mechanism consisting of geometric constraint initial screening and lightweight semantic segmentation fine screening, obtaining image data containing only static feature points; the VINS front-end module receives the static feature point image data output by the dynamic feature point filtering module and the inertial data output by the IMU data acquisition module, performs feature point matching, IMU pre-integration, and initial pose estimation, and obtains the pre-integration result and initial pose information; the VINS back-end optimization module establishes a nonlinear optimization problem of visual-inertial fusion based on the pre-integration result and initial pose information output by the VINS front-end module, and solves it using a sliding window optimization method to obtain a globally consistent carrier pose, i.e., the navigation and positioning result.

[0016] Furthermore, the aforementioned dynamic feature point filtering module and the VINS front-end module achieve parallel-synchronous integration through a caching unit and a timestamp synchronization mechanism. The synchronization mechanism includes:

[0017] After the image acquisition module outputs an image frame, it simultaneously transmits the image data to the buffer unit of the dynamic feature point filtering module and the VINS front-end module; the dynamic feature point filtering module performs geometric constraint initial screening and lightweight semantic segmentation fine screening in parallel.

[0018] After receiving image data in the buffer unit, the VINS front-end module first performs IMU pre-integration and waits for the dynamic feature point filtering module to output the static feature point results. After the static feature point results are output, the VINS front-end module directly performs feature matching and initial pose estimation based on the static feature points.

[0019] During the integration process, the dynamic feature point filtering results are synchronized with the IMU data and image data through a timestamp synchronization mechanism. That is, a unified timestamp is added to each frame of image, IMU data and static feature point results, and the VINS front-end module only uses data within the same timestamp range for fusion calculation.

[0020] Furthermore, this invention also proposes a method for achieving robust visual inertial navigation based on the above system, comprising the following steps:

[0021] (1) Acquire continuous image frames of the carrier through a monocular camera and acquire inertial data of the carrier through an inertial measurement unit (IMU);

[0022] (2) Perform dynamic feature point filtering on the image frame to output image data containing only static feature points; the dynamic feature point filtering process adopts a two-stage filtering mechanism, including:

[0023] (2a) Geometric constraint screening: Based on the feature point matching results of two consecutive frames of images, the epipolar constraint is used to determine whether the feature points are dynamic and remove obviously dynamic feature points.

[0024] (2b) Lightweight semantic segmentation fine screening: The image region containing the feature points retained after the initial screening by geometric constraints is input into a pre-trained lightweight semantic segmentation network. Based on the semantic segmentation results output by the network, latent dynamic feature points located in dynamic regions are removed, including: if the feature point is located in a dynamic region in the semantic segmentation results, the feature point is determined to be a dynamic feature point and is removed; if it is located in a static region, it is determined to be a static feature point and is retained; finally, image data containing only static feature points is obtained.

[0025] (3) Input the image data containing only static feature points obtained in step (2) and the inertial data collected in step (1) into the VINS front-end module to perform feature point matching, IMU pre-integration and initial pose estimation, obtain the pre-integration result and initial pose information, and output them to the VINS back-end optimization module.

[0026] (4) The VINS back-end optimization module receives the pre-integration results and initial pose information, and obtains the final pose of the carrier, i.e. the navigation and positioning results, through nonlinear optimization.

[0027] Compared with the prior art, the present invention has the following advantages:

[0028] First, high accuracy in dynamic feature point filtering:

[0029] Because this invention employs a two-stage filtering mechanism of "geometric constraint initial screening + lightweight semantic segmentation fine screening," it not only quickly eliminates obvious dynamic feature points through epipolar constraints but also accurately identifies latent dynamic feature points through lightweight semantic segmentation. Compared to the traditional VINS-Mono scheme that relies solely on static feature point assumptions, the dynamic feature point elimination rate is increased to over 95%, effectively reducing the interference of the dynamic environment on navigation accuracy.

[0030] Second, it has low computational overhead and strong real-time performance:

[0031] The geometric constraint initial screening method used in this invention is based on optical flow and epipolar constraints, which has low computational complexity (single frame processing time <10ms); the lightweight semantic segmentation network adopts MobileNetV2 and depthwise separable convolution, with only 1 / 8 of the number of parameters of traditional semantic segmentation networks (such as DeepLabV3), and the inference time is also low. 30ms, total time for two-stage filtering With a 40ms response time, it can adapt to image acquisition frequencies of 20-30fps. At the same time, the module and the VINS front end adopt a "parallel-synchronous" integration method to avoid data redundancy. Its navigation output frequency is consistent with the image frame rate, ensuring the overall real-time performance of the system.

[0032] Third, it has strong compatibility and is easy to deploy:

[0033] The dynamic feature point filtering module of this invention serves as a front-end processing unit for VINS, eliminating the need to modify the back-end optimization logic of VINS-Mono. It can be directly integrated into existing VINS frameworks (such as VINS-Mono and VINS-Fusion), exhibiting strong hardware adaptability and stable operation on embedded platforms (such as NVIDIA Jetson TX2 and RK3588), significantly reducing deployment costs. Attached Figure Description

[0034] Figure 1This is a schematic diagram of the overall architecture of the system of the present invention;

[0035] Figure 2 This is a flowchart of the two-stage filtering mechanism of the dynamic feature point filtering module in this invention;

[0036] Figure 3 This is a schematic diagram of the lightweight semantic segmentation network in this invention. Detailed Implementation

[0037] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0038] Example 1: Refer to Figure 1 The present invention proposes a robust visual inertial navigation system based on dynamic feature point filtering, comprising an image acquisition module, an IMU data acquisition module, a dynamic feature point filtering module, a VINS front-end module, and a VINS back-end optimization module; wherein the image acquisition module, the dynamic feature point filtering module, the VINS front-end module, and the VINS back-end optimization module are connected sequentially, and the output of the IMU data acquisition module is connected to the input of the VINS front-end module;

[0039] The image acquisition module is used to acquire image data and output it to the dynamic feature point filtering module. In this embodiment, a monocular camera is used to acquire continuous image frames during the movement of the carrier as image data at a preset frame rate. The preset frame rate is preferably 20-30fps.

[0040] The IMU data acquisition module is used to acquire the inertial data of the carrier and output it to the VINS front-end module. The IMU device used in this embodiment includes an accelerometer and a gyroscope, and acquires the inertial data of the carrier at a frequency more than 10 times higher than the image frame rate. The frequency range is preferably 200-500Hz. The inertial data includes acceleration data acquired by the accelerometer and angular velocity data acquired by the gyroscope.

[0041] The dynamic feature point filtering module identifies and removes dynamic feature points in the image by performing a two-stage filtering mechanism on the image data, and outputs image data containing only static feature points to the VINS front-end module. In this embodiment, dynamic feature points in the image are identified and removed by a two-stage filtering mechanism consisting of geometric constraint initial screening and lightweight semantic segmentation fine screening, resulting in image data containing only static feature points.

[0042] The VINS front-end module is used to process the inertial data output by the IMU data acquisition module and the image data output by the dynamic feature point filtering module to obtain the pre-integration result and initial pose information, and output them to the VINS back-end optimization module. In this embodiment, the VINS front-end module receives the static feature point image data output by the dynamic feature point filtering module and the inertial data output by the IMU data acquisition module, performs feature point matching, IMU pre-integration, and initial pose estimation, and obtains the pre-integration result and initial pose information.

[0043] The VINS backend optimization module obtains the carrier pose through nonlinear optimization based on the output of the VINS frontend module. Specifically, it establishes a nonlinear optimization problem of visual-inertial fusion based on the pre-integration result and initial pose information output by the VINS frontend module, and solves it through a sliding window optimization method to obtain a globally consistent carrier pose, i.e., the navigation and positioning result.

[0044] In this embodiment, the dynamic feature point filtering module and the VINS front-end module achieve parallel-synchronous integration through a caching unit and a timestamp synchronization mechanism. The synchronization mechanism includes:

[0045] After the image acquisition module outputs an image frame, it simultaneously transmits the image data to the buffer unit of the dynamic feature point filtering module and the VINS front-end module. The dynamic feature point filtering module performs geometric constraint initial screening and lightweight semantic segmentation fine screening in parallel. In this embodiment, the total time for this process is preferably controlled to 30-40ms, which is lower than the image frame interval of 40-50ms.

[0046] After receiving image data in the buffer unit, the VINS front-end module first performs IMU pre-integration, which is based on high-frequency IMU data and takes a short time, usually 5-10ms. Then it waits for the dynamic feature point filtering module to output the static feature point results. After the static feature point results are output, the VINS front-end module directly performs feature matching and initial pose estimation based on the static feature points. There is no need to re-extract feature points, avoiding data redundancy and repeated calculations.

[0047] During integration, a timestamp synchronization mechanism ensures that the dynamic feature point filtering results are consistent with the IMU data and image data in terms of time. This means that a unified timestamp is added to each frame of image, IMU data, and static feature point results. The VINS front-end module only uses data within the same timestamp range for fusion calculations, avoiding errors caused by asynchronous time processing.

[0048] Example 2: The overall architecture of the robust visual inertial navigation system proposed in this example is the same as in Example 1. Refer to the following... Figure 2 and 3 The dynamic feature point filtering module in this invention will be described in further detail as follows:

[0049] The dynamic feature point filtering module involved in this invention employs a two-stage filtering mechanism to achieve accurate identification and removal of dynamic feature points while ensuring low computational overhead, including:

[0050] (a) Initial screening based on geometric constraints: quickly eliminate obvious dynamic feature points.

[0051] Get the current image frame (denoted as ) ) and the previous frame image (denoted as The FAST (Features from Accelerated Segment Test) feature point extraction was performed on the two frames of the image to obtain the results. feature point set and feature point set ;

[0052] Using the LK (Lucas-Kanade) optical flow method and Perform feature matching to obtain a set of matching feature point pairs. Determining the dynamic nature of matched feature point pairs based on epipolar constraints: Based on camera motion priors provided by IMU pre-integration, constraints are constructed using epipolar geometry principles. The position of a static feature point in the previous frame is constrained to fall near a specific straight line (epidural line) in the current frame. By calculating the distance from the feature point to the epipolar line, it can be determined whether its motion is consistent with the camera's own motion. For matched feature point pairs... ,Will The epipolar line is obtained by mapping through the fundamental matrix F. ,calculate to the poles distance ;like Preset threshold ( Based on camera resolution and noise level settings (usually 1-2 pixels), the feature point pair is determined to be a dynamic feature point and is discarded; if... If the feature point pair is selected, it will be retained and proceed to the next step of the fine screening process.

[0053] (ii) Lightweight semantic segmentation screening: accurately identify latent dynamic feature points.

[0054] A lightweight semantic segmentation network is constructed as a supplement to the initial screening of geometric constraints to identify latent dynamic feature points that cannot be eliminated by geometric constraints (such as feature points of dynamic objects with slow movement speed and epipolar distance meeting the threshold).

[0055] The lightweight semantic segmentation network adopts a structure of "MobileNetV2 as the backbone network + depthwise separable convolution to construct the decoding layer": the backbone network MobileNetV2 reduces the number of parameters while ensuring feature extraction capabilities through inverted residual blocks and linear bottleneck structures; the decoding layer uses depthwise separable convolution to replace traditional convolution, decomposing the convolution operation into depthwise convolution and pointwise convolution, with the number of parameters and computational cost being only a fraction of that of traditional convolution. ( The kernel size is usually set to 3, which effectively reduces the network inference overhead.

[0056] During network training, a training sample set is constructed using dynamic environment datasets (such as a subset of KITTI dynamic scenes and TUM RGB-D dynamic sequences). The sample labels only distinguish between two categories: "dynamic objects" and "static backgrounds" (rather than subcategories), reducing label annotation costs and network learning difficulty. The training objective function uses the cross-entropy loss function, and the optimizer uses the Adam optimizer. The initial learning rate is set to 1e-4, and it decays to 0.5 of the original rate every 10 epochs until training converges.

[0057] The image regions containing the feature points retained after the initial screening by geometric constraints are input into the trained lightweight semantic segmentation network. The network outputs the semantic segmentation results of the image (dynamic regions are marked as 1, and static regions are marked as 0). If a feature point is located in a dynamic region (marked as 1) in the semantic segmentation results, it is determined to be a dynamic feature point and is removed. If it is located in a static region (marked as 0), it is determined to be a static feature point, retained, and output to the VINS front-end module.

[0058] Example 3: The overall architecture of the robust visual inertial navigation system proposed in this example is the same as that in Example 1. The overall operation and pose output process of the VINS will now be described in further detail:

[0059] The image acquisition module and IMU data acquisition module acquire data in real time, while the dynamic feature point filtering module continuously outputs static feature points according to a two-stage filtering mechanism. The VINS front-end module receives static feature points and IMU data, performs feature matching (based only on static feature points to avoid dynamic interference), IMU pre-integration (calculating the initial constraints of the carrier motion based on IMU data), and initial pose estimation (solving the carrier pose by combining the PNP (Perspective-n-Point) algorithm with the IMU pre-integration results), and outputs the pre-integration results and initial pose information to the VINS back-end optimization module. The VINS back-end optimization module adopts a sliding window optimization strategy, with the window size set to 5-10 frames. The window contains the visual constraints of static feature points and the inertial constraints of IMU pre-integration. A nonlinear optimization problem is constructed through Bundle Adjustment to minimize the weighted sum of visual reprojection error and IMU pre-integration error, and the globally consistent carrier pose (position, attitude, velocity) is obtained. Finally, the VINS back-end optimization module outputs the real-time pose of the carrier at a preset frequency (consistent with the image frame rate, 20-30Hz) to complete the navigation and positioning function.

[0060] Example 4: Refer to Figure 1 This invention proposes a method for implementing robust visual inertial navigation based on the system described in Embodiment 1, specifically including the following steps:

[0061] Step 1) Acquire continuous image frames of the carrier using a monocular camera and acquire inertial data of the carrier using an inertial measurement unit (IMU).

[0062] Step 2) Perform dynamic feature point filtering on the image frame to output image data containing only static feature points; the dynamic feature point filtering process employs a two-stage filtering mechanism, including:

[0063] (2a) Initial screening based on geometric constraints: Based on the feature point matching results of two consecutive frames of images, the epipolar constraint is used to determine whether the feature points are dynamic and remove obviously dynamic feature points; In this embodiment, the FAST feature points are extracted from the current image frame and the previous image frame respectively, and feature matching is performed by the LK optical flow method to obtain matching feature point pairs; For each matching feature point pair, the distance from the feature point of the current frame to the epipolar line obtained by mapping the feature point of the previous frame through the fundamental matrix is ​​calculated; If the distance is greater than the preset threshold, the feature point is determined to be an obviously dynamic feature point and is removed.

[0064] (2b) Lightweight semantic segmentation fine screening: The image region containing the feature points retained after the initial screening by the geometric constraints is input into a pre-trained lightweight semantic segmentation network. Based on the semantic segmentation results output by the network, latent dynamic feature points located in dynamic regions are removed, including: if the feature point is located in a dynamic region in the semantic segmentation results, the feature point is determined to be a dynamic feature point and is removed; if it is located in a static region, it is determined to be a static feature point and is retained; finally, image data containing only static feature points is obtained.

[0065] In this embodiment, the pre-trained lightweight semantic segmentation network is used to identify latent dynamic feature points that cannot be eliminated by the initial screening of geometric constraints, including feature points of dynamic objects with slow movement speed and epipolar distance meeting the threshold. The network uses MobileNetV2 as the backbone network, and the decoding layer is composed of depthwise separable convolutions. The backbone network is implemented through inverted residual blocks and linear bottleneck structures. The decoding layer decomposes the convolution operation into depthwise convolution and pointwise convolution. The network training process uses a dynamic environment dataset to construct a training sample set, which includes a subset of KITTI dynamic scenes and TUM RGB-D dynamic sequences. The sample labels only distinguish between "dynamic objects" and "static backgrounds". The training objective function uses the cross-entropy loss function, the optimizer uses the Adam optimizer, the initial learning rate is set to 1e-4, and it decays to 0.5 of the original rate every 10 epochs until the training converges, resulting in a trained lightweight semantic segmentation network.

[0066] Step 3) Input the image data containing only static feature points obtained in Step 2) and the inertial data collected in Step (1) into the VINS front-end module to perform feature point matching, IMU pre-integration and initial pose estimation, obtain the pre-integration result and initial pose information, and output them to the VINS back-end optimization module; The above IMU pre-integration is based on the IMU data to calculate the initial constraints of the carrier motion, and the initial pose estimation is the process of solving the carrier pose by combining the PNP algorithm with the IMU pre-integration result.

[0067] Step 4) The VINS backend optimization module receives the pre-integration results and initial pose information, and obtains the final pose of the carrier, i.e., the navigation and positioning results, through nonlinear optimization.

[0068] In this embodiment, the VINS backend optimization module adopts a sliding window optimization strategy, preferably setting the window size to 5-10 frames, and the window includes visual constraints of static feature points and inertial constraints of IMU pre-integration. A nonlinear optimization problem is constructed using the bundle adjustment method to minimize the weighted sum of visual reprojection error and IMU pre-integration error, and the globally consistent carrier pose, including position, attitude, and velocity, is obtained. The real-time pose of the carrier is output at a preset frequency to complete navigation and positioning.

[0069] Example 5: The robust visual-inertial navigation system proposed in this example has the same structure as in Example 1, and the overall implementation steps of the navigation method are the same as in Example 4. The following detailed description is given for its specific application:

[0070] The electronic device in this embodiment includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the method of the present invention.

[0071] First, a robust VINS hardware and software environment based on dynamic feature point filtering is constructed. Overcoming the limitations of real-device experiments, this invention builds a test platform based on the Gazebo simulation environment under Ubuntu 18.04 and the ROS Melodic framework. The platform carries a quadcopter drone model, equipped with a simulated monocular camera (1280×720 resolution, 30fps) and a six-axis IMU (200Hz sampling frequency). Simulated indoor corridor and outdoor road scenes containing dynamic objects (such as pedestrians and vehicles) are constructed for algorithm verification. Software environment: Operating system is Ubuntu 18.04 LTS, deep learning framework is PyTorch 1.8.0, vision library is OpenCV 4.5.0, and the VINS basic framework is based on modified VINS-Mono open-source code.

[0072] Next, the parameters for the dynamic feature point filtering module are configured and the network is trained. The parameters of the dynamic feature point filtering module can be configured according to the actual application scenario. In the initial geometric constraint screening, the FAST feature point extraction threshold, LK optical flow window size, and epipolar distance threshold τ need to be adaptively set according to the resolution of the camera used and the image noise level. The lightweight semantic segmentation network needs to be trained using an image dataset containing dynamic objects. During training, a moderate learning rate can be used in conjunction with a learning rate decay strategy to avoid overfitting and ensure model convergence. Through reasonable parameter configuration and training, this module can effectively filter dynamic feature points and ensure the real-time processing capability of the entire system.

[0073] Next, the integration and debugging of the dynamic feature point filtering module with the VINS framework were carried out. During the integration and debugging phase of the dynamic feature point filtering module and the VINS framework, the consistency of the timestamps of the camera and IMU data was ensured by using a timestamp synchronization tool (such as ntpdate) on the Linux system, and the deviation between the output timestamp of the dynamic feature point filtering module and the image frame timestamp was controlled within ±5ms. Since the dynamic feature point filtering module has removed dynamic feature points, the feature point matching threshold of the VINS front end was adjusted from 0.8 in the traditional VINS-Mono to 0.7 to relax the matching conditions and ensure the number of static feature point matches. The window size of the IMU pre-integration remained unchanged at 50 frames, while the sliding window size of the back end was set to 8 frames. The ratio of the visual reprojection error weight to the IMU pre-integration error weight was set to 1:10 (this ratio was adjusted according to the reliability of static feature points). After the integration and debugging were completed, the present invention and the traditional VINS-Mono were tested in a dynamic environment (such as a campus road with pedestrians, bicycles, and cars, or an indoor corridor with walking people and moving shelves). Navigation accuracy and real-time performance were tested, with test metrics including pose error (position error RMSE, attitude error RMSE), navigation output frequency, and single-frame processing time.

[0074] Finally, after completing system integration and debugging, the effectiveness of the present invention was verified. Testing was conducted in the constructed Gazebo dynamic simulation environment. Based on the core technical principles and module design of this invention, and referring to successful solutions already verified in the prior art, the simulation results are analyzed as follows:

[0075] Regarding navigation accuracy: Existing technologies demonstrate that introducing a dynamic feature removal mechanism into the VINS framework can significantly improve positioning accuracy. For example, a dynamic feature removal method combining structural similarity networks and optical flow filtering has reduced the position error to 35.5% of that of traditional VINS on the EuRoC dataset. This invention, based on the same technological paradigm, employs a two-stage filtering mechanism of "geometric constraint initial screening + lightweight semantic segmentation fine screening," which is expected to effectively remove the vast majority of dynamic feature points. Therefore, it can be reasonably inferred that in dynamic environments, the root mean square error (RMSE) of pose estimation in this invention is expected to be significantly lower than that of traditional VINS-Mono.

[0076] Regarding system robustness: Research shows that strategies for classifying and processing based on the dynamic level of the scene (such as dynamic judgment methods combining GMS feature matching and chi-square test) can significantly enhance the system's adaptability and robustness in complex indoor and outdoor environments. This invention draws upon this concept of "scene-specific differentiated processing," where geometric constraints can quickly respond to obvious moving targets, while semantic screening can effectively handle slow or temporarily stationary dynamic objects. Therefore, this invention is expected to exhibit more stable pose output performance than traditional methods when facing dynamic disturbances at different speeds.

[0077] Regarding real-time performance: Existing solutions have verified that employing a lightweight object detection model is key to balancing the accuracy of dynamic object recognition with the computational overhead of the system. The lightweight semantic segmentation network used in this invention, built on MobileNetV2 and depthwise separable convolutions, has significantly lower parameter and computational costs than traditional semantic segmentation models. Combined with a "parallel-synchronous" integration mechanism specifically designed for real-time performance, the processing time of the entire dynamic feature point filtering module is expected to be stably controlled within 40ms, thus fully adapting to an image acquisition frequency of 20-30fps. This demonstrates that this invention successfully ensures the real-time performance of the VINS system while improving accuracy.

[0078] In summary, existing technologies have demonstrated the effectiveness and superiority of the technical approach adopted in this invention from various perspectives. Therefore, it is reasonable to expect that the technical solution of this invention will comprehensively outperform the traditional VINS-Mono system in terms of navigation accuracy, environmental robustness, and real-time performance in simulation experiments, effectively solving the core pain points of visual-inertial navigation in dynamic environments.

[0079] This invention can be widely applied in fields requiring high-precision real-time positioning and navigation in dynamic and complex environments, such as drone-based urban logistics, autonomous driving, warehouse robots, and AR / VR. Its technological value lies in its ability to significantly improve the operational reliability of intelligent equipment in the real dynamic world. Specifically, in drone logistics and inspection, it effectively addresses dynamic interference from birds and vehicles, ensuring flight safety and path stability; in autonomous driving scenarios, it ensures vehicle positioning does not drift due to interference from pedestrians or other vehicles, improving driving safety; in warehousing and service robot applications, it enables stable and smooth autonomous navigation even in environments with frequent personnel movement; and in mobile AR / VR devices, it provides more stable and low-latency pose tracking, significantly improving the immersive interactive experience. Furthermore, this technology also holds great potential in extreme operational environments such as emergency rescue. As intelligent equipment continues to move from the laboratory to the real open world, this invention provides crucial support for solving the core challenge of "dynamic environment perception and positioning," resulting in a very broad market prospect.

[0080] The parts of this invention not described in detail are common knowledge to those skilled in the art.

[0081] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Obviously, those skilled in the art, after understanding the content and principle of the present invention, may make various modifications and changes in form and detail without departing from the principle and structure of the present invention. However, these modifications and changes based on the concept of the present invention are still within the scope of protection of the claims of the present invention.

Claims

1. A robust visual-inertial navigation system based on dynamic feature point filtering, characterized in that, It includes an image acquisition module, an IMU data acquisition module, a dynamic feature point filtering module, a VINS front-end module, and a VINS back-end optimization module; wherein the image acquisition module, the dynamic feature point filtering module, the VINS front-end module, and the VINS back-end optimization module are connected in sequence, and the output of the IMU data acquisition module is connected to the input of the VINS front-end module; The image acquisition module is used to acquire image data and output it to the dynamic feature point filtering module; The IMU data acquisition module is used to acquire the inertial data of the carrier and output it to the VINS front-end module. The dynamic feature point filtering module identifies and removes dynamic feature points in the image by performing a two-stage filtering mechanism on the image data, and outputs image data containing only static feature points to the VINS front-end module. The VINS front-end module is used to process the inertial data output by the IMU data acquisition module and the image data output by the dynamic feature point filtering module to obtain the pre-integration result and initial pose information, and output them to the VINS back-end optimization module. The VINS back-end optimization module obtains the carrier pose through nonlinear optimization based on the output of the VINS front-end module.

2. The system according to claim 1, characterized in that: The image acquisition module uses a monocular camera to acquire continuous image frames during the carrier's motion at a preset frame rate as image data. The IMU data acquisition module uses an IMU device including an accelerometer and a gyroscope to acquire inertial data of the carrier at a frequency more than 10 times higher than the image frame rate. The inertial data includes acceleration data acquired by the accelerometer and angular velocity data acquired by the gyroscope. The dynamic feature point filtering module identifies and removes dynamic feature points in the image through a two-stage filtering mechanism consisting of geometric constraint initial screening and lightweight semantic segmentation fine screening, resulting in image data containing only static feature points. The VINS front-end module receives static feature point image data output by the dynamic feature point filtering module and inertial data output by the IMU data acquisition module, performs feature point matching, IMU pre-integration, and initial pose estimation, and obtains pre-integration results and initial pose information. The VINS backend optimization module establishes a nonlinear optimization problem of visual-inertial fusion based on the pre-integration results and initial pose information output by the VINS frontend module, and obtains the globally consistent carrier pose, i.e., the navigation and positioning result, by solving the problem through the sliding window optimization method.

3. The system according to claim 2, characterized in that: The dynamic feature point filtering module and the VINS front-end module achieve parallel-synchronous integration through a caching unit and a timestamp synchronization mechanism.

4. The system according to claim 3, characterized in that: The synchronization mechanism includes: After the image acquisition module outputs an image frame, it simultaneously transmits the image data to the buffer unit of the dynamic feature point filtering module and the VINS front-end module; the dynamic feature point filtering module performs geometric constraint initial screening and lightweight semantic segmentation fine screening in parallel. After receiving image data in the buffer unit, the VINS front-end module first performs IMU pre-integration and waits for the dynamic feature point filtering module to output the static feature point results. After the static feature point results are output, the VINS front-end module directly performs feature matching and initial pose estimation based on the static feature points. During the integration process, the dynamic feature point filtering results are synchronized with the IMU data and image data through a timestamp synchronization mechanism. That is, a unified timestamp is added to each frame of image, IMU data and static feature point results, and the VINS front-end module only uses data within the same timestamp range for fusion calculation.

5. A method for implementing robust visual-inertial navigation based on the system described in claim 1, characterized in that, Includes the following steps: (1) Acquire continuous image frames of the carrier through a monocular camera and acquire inertial data of the carrier through an inertial measurement unit (IMU); (2) Perform dynamic feature point filtering on the image frame and output image data containing only static feature points; The dynamic feature point filtering process employs a two-stage filtering mechanism, including: (2a) Geometric constraint screening: Based on the feature point matching results of two consecutive frames of images, the epipolar constraint is used to determine whether the feature points are dynamic and remove obviously dynamic feature points. (2b) Lightweight semantic segmentation fine screening: The image region containing the feature points retained after the initial screening by geometric constraints is input into a pre-trained lightweight semantic segmentation network. Based on the semantic segmentation results output by the network, latent dynamic feature points located in dynamic regions are removed, including: if the feature point is located in a dynamic region in the semantic segmentation results, the feature point is determined to be a dynamic feature point and is removed; if it is located in a static region, it is determined to be a static feature point and is retained; finally, image data containing only static feature points is obtained. (3) Input the image data containing only static feature points obtained in step (2) and the inertial data collected in step (1) into the VINS front-end module to perform feature point matching, IMU pre-integration and initial pose estimation, obtain the pre-integration result and initial pose information, and output them to the VINS back-end optimization module. (4) The VINS back-end optimization module receives the pre-integration results and initial pose information, and obtains the final pose of the carrier, i.e. the navigation and positioning results, through nonlinear optimization.

6. The method according to claim 5, characterized in that: The geometric constraint screening in step (2a) is specifically implemented as follows: FAST feature point extraction is performed on the current image frame and the previous image frame respectively, and feature matching is performed by LK optical flow method to obtain matching feature point pairs; For each matching feature point pair, calculate the distance from the current frame feature point to the epipolar line obtained by mapping the previous frame feature point through the fundamental matrix; if the distance is greater than a preset threshold, the feature point is determined to be an obvious dynamic feature point and is removed.

7. The method according to claim 5, characterized in that: The pre-trained lightweight semantic segmentation network described in step (2b) is used to identify latent dynamic feature points that cannot be eliminated by the initial screening of geometric constraints, including feature points of dynamic objects with slow movement speed and epipolar distance meeting the threshold. The network uses MobileNetV2 as the backbone network, and the decoding layer is composed of depthwise separable convolutions. The backbone network is implemented through inverted residual blocks and linear bottleneck structures. The decoding layer decomposes the convolution operation into depthwise convolution and pointwise convolution. The network training process uses a dynamic environment dataset to construct a training sample set, which includes a subset of KITTI dynamic scenes and TUM RGB-D dynamic sequences. The sample labels only distinguish between "dynamic objects" and "static backgrounds". The training objective function uses the cross-entropy loss function, the optimizer uses the Adam optimizer, the initial learning rate is set to 1e-4, and it decays to 0.5 of the original rate every 10 epochs until the training converges, resulting in a trained lightweight semantic segmentation network.

8. The method according to claim 5, characterized in that: Step (3) involves IMU pre-integration to calculate the initial constraints of the carrier motion based on IMU data. The initial pose estimation is the process of solving the carrier pose by combining the PNP algorithm with the IMU pre-integration results.

9. The method according to claim 5, characterized in that: The VINS backend optimization module in step (4) adopts a sliding window optimization strategy, sets the window size to 5-10 frames, and includes visual constraints of static feature points and inertial constraints of IMU pre-integration within the window. It constructs a nonlinear optimization problem through the bundle adjustment method, minimizes the weighted sum of visual reprojection error and IMU pre-integration error, and solves to obtain a globally consistent carrier pose, including position, attitude, and velocity. It then outputs the real-time pose of the carrier at a preset frequency to complete navigation and positioning.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, it implements the method as described in any one of claims 5 to 9.

Images