A long-range and long-duration integrated navigation method and system combining visual-inertial joint optimization and image matching

Abstract

The present invention belongs to the technical field of autonomous navigation and positioning of unmanned aerial vehicles (UAVs), and relates to a long-range, long-duration combined navigation method and system combining visual-inertial joint optimization with image matching. The method comprises: step 1: obtaining an offline map of the surrounding environment of the UAV flight path by querying satellite information, and presetting a number of waypoints in the mission path according to the map conditions; step 2: performing high-frequency relative navigation between the waypoints using visual-inertial navigation to achieve continuous pose estimation; step 3: triggering a switch in navigation strategy when approaching a waypoint based on a constructed multimodal navigation autonomous decision-making scheme, and switching to image matching navigation; step 4: implementing image matching navigation using a latent diffusion model and an XFeat model based on real-time photos taken by a monocular camera, and correcting the relative errors accumulated between the waypoints; and step 5: repeating steps 2 to 4 between multiple waypoints until the mission path is traversed.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous navigation and positioning of unmanned aerial vehicles (UAVs), and in particular relates to a long-range and long-duration combined navigation method and system combining visual-inertial joint optimization and image matching. Background Art

[0002] Autonomous drone navigation and positioning technology uses sensor data to determine a drone's position and attitude in completely unknown environments, providing the data foundation for subsequent map building and navigation tasks. Autonomous positioning is not only a core component of unmanned systems but also crucial for ensuring their robust operation in complex environments.

[0003] Traditional unmanned systems often use a fusion of satellite and inertial navigation. However, satellite positioning systems are significantly affected by the environment. When operating in valleys, buildings, or indoor environments, satellite signals are easily blocked or subject to multipath effects. In wartime, signals are also susceptible to shielding or interference. When satellite positioning fails, inertial navigation alone is susceptible to sensor errors, system noise, and integrated cumulative errors. After prolonged use, position and velocity information drifts, requiring additional time to correct for these accumulated errors, leading to navigation system instability and inaccuracy.

[0004] Pure visual positioning was proposed by Stanford University as early as the 1980s. However, due to limitations in sensor accuracy and processor performance, it remained confined to the experimental research stage for a long time. With the advancement of electronic information technology in the 21st century, vision-based positioning has made significant progress. Currently, there are two main approaches to vision-based navigation: relative navigation and absolute navigation. Relative navigation, a SLAM technology, does not rely on a reference image library and can autonomously calculate relative pose based on the correlation of two image frames. However, it suffers from error accumulation. Absolute navigation, on the other hand, uses image matching to locate drones. It can calculate absolute pose based on a reference image library without error accumulation, but it requires the storage of a reference image library, making it difficult to implement for long-endurance drone deployments and requiring large amounts of storage.

[0005] Visual / inertial sensor fusion is currently a common approach for local relative navigation (SLAM). The earliest visual / inertial system was proposed in 2007 by Anastasios Mourikis et al. at the University of California, Riverside. Subsequently, a series of classic algorithms emerged, including VI-ORB, VINS-Mono, and ORB-SLAM3. VI-ORB (Visual-Inertial ORB-SLAM), proposed in 2017, is a visual-inertial fusion system based on the ORB-SLAM framework. It tightly couples IMU data with ORB visual features. Its core improvement lies in improving trajectory accuracy by jointly optimizing visual reprojection error and IMU measurement error. However, this method still has certain limitations: 1) It is sensitive to temporal synchronization and calibration errors between the IMU and camera; even small deviations can lead to cumulative errors; 2) It does not explicitly handle dynamic objects and relies on a static environment assumption, which can interfere with localization results. VINS-Mono, proposed the same year, is a representative framework for monocular visual-inertial SLAM. It employs a tightly coupled optimization strategy to fuse IMU and monocular image data. Its innovation lies in balancing accuracy and efficiency through sliding window optimization combined with global pose graph adjustment. Its main drawbacks include: 1) the lack of an integrated dynamic object detection module, which can lead to localization failure in dynamic scenes; 2) loop closure detection relies on a bag-of-visual-words model, which is prone to mismatching when dealing with repetitive textures or changing viewpoints. ORB-SLAM3, introduced in 2020, is the third generation of the ORB-SLAM series and supports multimodal input (monocular, binocular, RGB-D, and visual-inertial). Its core improvements include: 1) the introduction of a multi-map system (Atlas), enabling cross-scene map fusion and long-term localization; 2) an optimized visual-inertial initialization process to improve convergence speed and robustness. However, challenges remain: 1) limited dynamic environment processing capabilities, the continued use of static assumptions, and the lack of explicit removal of dynamic features; 2) divergence caused by the long-term accumulation of visual-inertial mode errors; and 3) insufficient map density, requiring the reliance on extension modules to build practical-quality maps. The above analysis demonstrates that visual-inertial navigation solutions inevitably face the problem of error accumulation in long-duration, cross-view, multi-scene scenarios.

[0006] The image matching method for absolute navigation originated in the terminal guidance of cruise missiles and later gradually developed into a visual navigation technology. Scene matching navigation systems (SMNS) have the characteristics of simple equipment structure, passive operation, and high positioning accuracy. They use image sensors to obtain images of the area near the flight or target area and match them with stored reference images to obtain aircraft position data. Figure 1As shown in the figure, after obtaining the destination, preliminary path planning is performed using satellite and aerial images. An adaptation zone along the planned route is then selected and calibrated to obtain a reference map. The real-time image acquired by the visual sensor is then matched with the reference map to achieve navigation and positioning. The core of this approach is the image matching algorithm, whose performance directly determines the performance of the navigation system. Image matching algorithms can be categorized based on different visual feature extraction methods: template matching-based methods, local invariant feature-based methods, and scene semantic learning-based methods. The template matching-based UAV scene matching and positioning algorithm, one of the earliest representative algorithms for UAV visual positioning, utilizes image information such as pixel intensity to construct a matching model to perform scene matching. Compared to template matching-based image matching algorithms, those based on local invariant features (also known as handcrafted features) exhibit stronger environmental adaptability. Local invariant features, also known as handcrafted feature descriptors, typically rely on expert prior knowledge during algorithm design. This knowledge complements these descriptors, resulting in superior performance in many applications compared to template matching methods. With the advancement of computer vision, deep neural networks, with their ability to capture high-dimensional semantic features in images, have led to the development of scene semantic learning-based methods. Compared to template matching and local invariant feature methods, image matching methods based on scene semantic learning have the advantage of deep neural networks' ability to extract semantic information, enabling better image matching relationships. Furthermore, by leveraging this semantic information, image matching algorithms can handle more complex viewpoint variations and overcome various potential interferences, such as illumination variations and noise. As a representative example of multi-view scene matching algorithms based on metric learning, Zheng et al. released the first multi-source, multi-view scene matching dataset for drone visual localization, University-1652, providing support for multi-view scene matching research. Based on this dataset, the authors constructed a three-branch Siamese neural network model to perform scene matching by establishing correspondences between multi-view drone imagery, street view imagery, and satellite imagery. The feature extraction model employed the classic ResNet architecture without additional enhancements. To address viewpoint discrepancies in image matching in drone visual localization algorithms, Yang et al. exploited the positional encoding of features. Using this positional encoding information, they guided a two-branch Siamese neural network to establish regional correspondences between multi-view image pairs. Position encoding allows image perspective relationships to be labeled, allowing the network to learn corresponding regions after perspective changes, improving the algorithm's multi-view robustness. However, the choice of the Transformer as the backbone feature extraction network and the spatial position correspondence module make the network relatively complex, presenting certain limitations in both training and mobile platform deployment.Wang et al. proposed a local pattern network (LPN) that uses a square ring feature partitioning strategy to extract features describing the central target and surrounding environment. By repeatedly measuring corresponding feature pairs, similarity results based on the central and edge square rings are obtained, which are ultimately fused to form a similarity metric. However, during the image region partitioning process, the ring partitioning pattern used in the similarity metric is completely fixed, making it difficult for the algorithm to self-adjust and make optimal decisions in different situations. From the above description of the image matching method based on scene semantic learning, it is clear that as a feature matching method based on deep learning, this method can effectively handle complex scenes and has excellent generalization and broad prospects. However, the method has the disadvantage of placing high demands on hardware conditions and offline map datasets for the scene, and it cannot achieve a balance between model lightweightness and matching performance.

[0007] In short, in the autonomous navigation and positioning of drones, long-distance and long-duration missions often face two core problems: first, in an environment without GNSS signals, visual inertial navigation accumulates errors, resulting in a significant decrease in positioning accuracy as the range increases; second, although traditional positioning methods based on image matching can provide absolute position correction, their reliance on high-quality reference maps throughout the entire process makes deployment difficult and real-time performance poor, and it is difficult to work stably when the reference maps in some areas are missing or change. Summary of the Invention

[0008] The purpose of the present invention is to overcome the defects of the prior art and propose a long-range and long-duration integrated navigation method and system combining visual-inertial joint optimization and image matching.

[0009] In view of this, the present invention proposes a long-range and long-duration integrated navigation method that combines visual-inertial joint optimization with image matching. The method is implemented based on a monocular camera and an IMU, and includes:

[0010] Step 1: Obtain an offline map of the drone's flight path and surrounding environment by querying satellite information, and preset several waypoints in the mission path based on the map.

[0011] Step 2: Use visual inertial navigation for high-frequency relative navigation between waypoints to achieve continuous pose estimation;

[0012] Step 3: Based on the constructed multimodal navigation autonomous decision-making scheme, the navigation strategy is switched to image matching navigation when approaching a waypoint.

[0013] Step 4: Based on the real-time photos taken by the camera, the Latent Diffusion model and the XFeat model are used to implement image matching navigation and correct the relative errors accumulated between waypoints.

[0014] Step 5: Repeat steps 2 to 4 between multiple waypoints until the mission path is traversed.

[0015] Preferably, the step 2 comprises:

[0016] Step 2-1: Pre-integrate the IMU data collected by the IMU and construct the pre-integration error term;

[0017] Step 2-2: For the real-time image captured by the monocular camera, select key frames and process them using the minimum photometric error direct method to obtain the image photometric errors of adjacent key frames;

[0018] Step 2-3: Perform joint error optimization on the IMU pre-integration error term and the image photometric error to obtain more accurate camera pose information.

[0019] Preferably, the pre-integration error term constructed in step 2-1 is For the time interval The difference between the predicted value and the actual observed value satisfies the following formula:

[0020]

[0021] in, yes The logarithmic mapping of is the Lie algebra vector extraction operation, , , They are respectively from Time has come Pre-integrated rotation, position and velocity increments at time, , , , , , , respectively represent the IMU in the world coordinate system Moment and The rotation, position and velocity at each moment, is the gravitational acceleration vector in the global coordinate system, is the time interval, and the superscript T indicates the transpose.

[0022] Preferably, the step 2-2 includes:

[0023] Calculate the difference in the gradient magnitude matrix between any two adjacent frames of images;

[0024] Each frame image is divided into 32×32 grids, and each grid retains the point with the largest difference in gradient amplitude between adjacent frames as the primary image key frame;

[0025] Dynamically eliminate image key frames with insufficient stability to obtain several key frames;

[0026] For adjacent keyframes after dynamic culling , the corresponding image photometric error is obtained according to the following formula :

[0027]

[0028] in, represents the grayscale value of the image, Keyframe The pixel coordinates of Project the pixel to the current keyframe through pose transformation 's coordinates.

[0029] Preferably, the steps 2-3 include:

[0030] The IMU pre-integration error term and image photometric error are unified into a nonlinear least squares problem. The sliding window optimization method is used to marginalize the old poses and map points. The positions of nearby map points are updated through new observations to obtain more accurate camera pose information.

[0031] Preferably, the multimodal navigation autonomous decision in step 3 includes:

[0032] A hierarchical feature pyramid is used to extract the semantic feature vectors and geometric feature vectors of the environment in the current map, and the extracted feature vectors are normalized and fused;

[0033] A similarity evaluation algorithm is used to calculate the cosine similarity between the fused feature vector and the pre-stored target area features. When the similarity of a set number of consecutive frames exceeds the threshold, the navigation strategy is switched to the image matching navigation method.

[0034] Preferably, the hierarchical feature pyramid includes a bottom layer and a top layer, wherein:

[0035] The bottom layer uses ORB to extract geometric features including edges and corners, and the top layer uses SIFT to extract semantic objects including building outlines and texture areas.

[0036] Preferably, step 4 includes:

[0037] Convert real-time photos taken by the camera into gallery-style images through the trained Latent Diffusion model;

[0038] Use the trained XFeat model to match the style-converted real-time images taken around the waypoints with the offline library photos, predict the pose offset, and achieve pixel-level matching through classification.

[0039] Calculate the current UAV posture using the matching results;

[0040] Using the sliding window optimization method, the old poses and map points are marginalized, and the positions of nearby map points are updated through new observations to achieve map point updates, thereby correcting the accumulated relative errors between waypoints.

[0041] On the other hand, the present invention provides a long-range and long-endurance integrated navigation system that combines visual-inertial joint optimization with image matching, which is implemented based on a monocular camera and an IMU. The system includes: a data acquisition module, a visual-inertial navigation module, a multimodal navigation autonomous decision module, and an image matching navigation module; wherein,

[0042] The data acquisition module is used to obtain an offline map of the environment surrounding the UAV flight path by querying satellite information, and preset a number of waypoints in the mission path according to the map conditions;

[0043] The visual inertial navigation module is used to perform high-frequency relative navigation between waypoints using visual inertial navigation to achieve continuous pose estimation;

[0044] The multimodal navigation autonomous decision-making module is used to trigger the switching of navigation strategies when approaching a waypoint based on the constructed multimodal navigation autonomous decision-making scheme, and switch to using the image matching navigation module;

[0045] The image matching navigation module is used to implement image matching navigation based on real-time photos taken by the camera using the Latent Diffusion model and the XFeat model to correct the relative errors accumulated between waypoints;

[0046] The visual inertial navigation module, multimodal navigation autonomous decision module, and image matching navigation module are reused between multiple waypoints until the mission path is traversed.

[0047] Compared with the prior art, the advantages of the present invention are:

[0048] 1. Image matching navigation method based on Latent Diffusion and XFeat model

[0049] Traditional image matching methods directly match the captured real-time image with pre-stored target image features and then calculate the pose from them. Although this positioning method can provide absolute position correction, its reliance on high-quality reference images throughout the process makes it difficult to deploy and has poor real-time performance. In addition, this method requires high accuracy of the image matching model. This method first converts the camera image into a gallery-style image using the LatentDiffusion model, and then uses the recently proposed XFeat model for feature extraction and matching. The Latent Diffusion model migrates the diffusion process to a low-dimensional latent space, preserving the quality of the generated image while maintaining processing efficiency. The XFeat lightweight model ensures good feature matching accuracy by adopting a new strategy of "doubling the convolution depth of the network when the input image resolution is halved."

[0050] 2. Multimodal navigation autonomous switching strategy

[0051] A strategy was developed to autonomously switch from visual-inertial navigation to image matching navigation. First, a hierarchical feature pyramid (ORB + SIFT combination) was used to extract the semantic and geometric features of the current environment. Second, a similarity evaluation algorithm was used to calculate the cosine similarity between the current camera's real-time environmental features and the pre-stored target area features. When the similarity exceeded a threshold, the navigation strategy was switched to image matching navigation.

[0052] 3. Combined navigation solution based on visual inertial navigation and image matching method

[0053] This paper proposes a combined navigation strategy that uses visual-inertial navigation between aircraft waypoints and image matching navigation near waypoints. This approach effectively compensates for the relative error accumulation caused by long-term visual-inertial navigation. For image matching navigation algorithms, limiting the application area near waypoints significantly reduces the dataset requirements. The distinct features surrounding waypoints also allow for feature matching using a lightweight model. These unique advantages complement the highly mature visual-inertial navigation system, further improving positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 This is the structural diagram of the image matching navigation system;

[0055] Figure 2 This is a technical solution framework diagram of the long-range and long-endurance integrated navigation system that combines visual-inertial joint optimization with image matching.

[0056] Figure 3 This is a diagram describing the technical solution of the long-range and long-endurance integrated navigation method combining visual-inertial joint optimization and image matching of the present invention;

[0057] Figure 4 This is the framework diagram of the visual inertial navigation solution;

[0058] Figure 5 It is the Latent Diffusion model training process. DETAILED DESCRIPTION

[0059] The present invention designs a combined navigation solution that combines visual inertial navigation and image matching: between waypoints, visual inertial navigation is used for high-frequency relative navigation to achieve continuous, low-drift pose estimation; and when approaching a waypoint, an image matching method is introduced to correct the current navigation state and correct the relative errors accumulated between waypoints. In this way, the advantages of the two technologies are effectively integrated, which not only ensures the continuity and high accuracy of positioning in the middle section of the voyage, but also significantly reduces the dependence on a complete reference map for the entire voyage. This solution improves the adaptability and reliability of the system in complex, non-cooperative environments, especially when the reference map is partially missing or changed, it can still maintain the continuity and stability of navigation accuracy, thereby significantly improving the positioning accuracy of the entire mission voyage.

[0060] The present invention uses a monocular camera and an IMU as external hardware devices to support the algorithm framework implementation, including the following steps:

[0061] Step 1: Build the data acquisition process. This involves map acquisition and waypoint presetting. By querying satellite information, the team obtains an offline map of the drone's flight path and surroundings, and then presetting several waypoints in the mission path based on the map.

[0062] Step 2: Construct the visual inertial navigation link. Visual inertial navigation is used between waypoints. The IMU pre-integration error term and the image photometric error are assigned corresponding weights to form a joint error term. All state parameters are then jointly optimized using a nonlinear optimization method.

[0063] Step 3: Build a multimodal navigation autonomous decision-making process. This process uses a hierarchical feature pyramid (ORB + SIFT combination) to extract the semantic and geometric features of the environment from the pre-stored offline map. A similarity evaluation algorithm is then used to calculate the cosine similarity between the real-time environment features captured by the current camera and the pre-stored target area features. When the similarity exceeds a threshold, the navigation strategy is switched to image matching navigation.

[0064] Step 4: Construct the image matching navigation link. Use the Latent Diffusion model to convert the real-time photos taken by the camera into gallery-style images. Then use XFeat to perform feature matching between the style-converted real-time images taken around the waypoints and the offline gallery photos to calculate the current drone's position. Through the above technical means, the relative error generated by visual inertial navigation is corrected to achieve improved positioning accuracy of the drone in multi-scene long-duration navigation tasks. The overall structural framework of the technical solution is detailed in Figure 2 , a graphic description of the scheme can be found in Figure 3 .

[0065] The technical solution of the present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0066] Example 1

[0067] The present invention proposes a long-range, long-duration integrated navigation method that combines visual-inertial joint optimization with image matching. The method includes the following parts:

[0068] Step 1: Construct data acquisition link;

[0069] Step 2: Build the visual inertial navigation link;

[0070] The visual inertial navigation solution specifically includes the following steps:

[0071] ① Inertial navigation branch. Pre-integrate the IMU and construct the pre-integration error term. First, integrate the gyroscope angular velocity and accelerometer data in the local coordinate system within the continuous time window to construct the relative motion increment. Secondly, model the attitude change through the Lie group SO(3) to compensate for the zero bias drift. Then convert the integration result into discrete posture constraints to form a pre-integration observation model and construct the pre-integration error term. Finally, the Jacobian matrix and covariance are passed to the backend optimizer for efficient state update.

[0072] ② Visual navigation branch. The real-time pictures taken by the camera are processed using the direct method of minimum photometric error. First, it is necessary to ensure that the camera is calibrated and the internal parameters of the camera (such as focal length, principal point coordinates, distortion coefficient, etc.) are determined to ensure the effectiveness of the photometric error and the performance of the algorithm in subsequent calculations. Secondly, the image key frames are extracted. The captured image is convolved with the Sobel / Prewitt operator to calculate the gradient amplitude of each pixel. The image is divided into 32×32 grids, and the point with the first gradient amplitude is retained in each grid as the preliminarily selected image key frame. After the preliminarily selected, the image key frames with insufficient stability are dynamically eliminated. Finally, the photometric error between adjacent frames is calculated. .

[0073] ③ Joint error term construction and optimization. Through the above two branches, the photometric error and pre-integration error terms are constructed respectively, and the joint error optimization of the back-end is performed, such as Figure 4 As shown in the figure, the core idea of ​​joint optimization is to dynamically balance the constraint weights of the two sensors and adjust all state parameters through nonlinear optimization. After joint optimization, more accurate camera pose information is obtained. Specifically, a sliding window optimization method is used to marginalize old poses and map points. New observations are used to update the positions of nearby map points, providing more accurate feature constraints for subsequent frames, further improving the system's positioning accuracy. Finally, the coordinates and covariance matrix of the map points are recalculated using the optimized poses.

[0074] 1.1 Constructing IMU pre-integration error term

[0075] In the time interval Internal, IMU pre-integrated components , , , respectively, representing Time has come The pre-integrated rotation, position, and velocity increments at time instant are defined as:

[0076]

[0077] in is to measure angular velocity and acceleration, is the gyroscope and accelerometer bias. is the IMU sampling interval, It is Lie Group The exponential map of .

[0078] Bias compensation and error propagation:

[0079] When the bias estimate changes slightly When , the correction formula of the pre-integration component is:

[0080]

[0081] The Jacobian matrix is ​​calculated by recursive update:

[0082]

[0083] Pre-integrated error term construction:

[0084] In the IMU backend optimization, the pre-integration error is defined as the difference between the predicted value and the actual observed value:

[0085]

[0086] in yes The logarithmic mapping of is the Lie algebra vector extraction operation, , , , , , Respectively represent the IMU in the world coordinate system Moment and The rotation, position and velocity at each moment, is the gravitational acceleration vector in the global coordinate system.

[0087] 1.2 Constructing image photometric error terms

[0088] Before calculating the photometric error, keyframe selection is performed to select the most representative frames from the video sequence to summarize the video content or mark significant scene changes. Keyframe selection depends on the gradient change strength of adjacent frames and is achieved through the following steps:

[0089] First, calculate the adjacent frames and The gradient magnitude matrix difference of :

[0090]

[0091] in, is the image resolution. The larger the difference value, the more significant the scene change. The gradient amplitude of each pixel Secondly, the image is divided into 32×32 grids, and the point with the highest gradient magnitude in each grid is retained as the initial image keyframe. Finally, the image keyframes with insufficient stability are dynamically eliminated.

[0092] Assuming adjacent keyframes The pixel intensity of the same scene point remains unchanged, and the photometric error is defined as:

[0093]

[0094] in represents the grayscale value of the image, for Pixel coordinates at the moment, The pixel is projected to the current frame through pose transformation 's coordinates.

[0095] 1.3 Joint Optimization

[0096] Unify the IMU pre-integration error term and the photometric error term into a nonlinear least squares problem:

[0097]

[0098] in is the robust kernel function, is the covariance matrix, and X is the optimization variable (pose, velocity, IMU bias, etc.). A sliding window optimization method is used to marginalize the old poses and map points, and update the positions of nearby map points with new observations, thereby providing more accurate feature constraints for subsequent frames.

[0099] Step 3: Construct multimodal navigation autonomous decision-making link;

[0100] The multimodal navigation autonomous decision-making solution specifically includes the following steps:

[0101] ① Multimodal environmental feature extraction. A hierarchical feature pyramid (ORB + SIFT combination) is used to extract low-level geometric features (edges, corners) and high-level semantic features (scenery, building outlines) of the environment in the pre-stored offline map.

[0102] ② Feature matching and trigger criterion generation: Perform real-time feature similarity evaluation and calculate the cosine similarity between the current environment features and the pre-stored target area features. When the matching degree exceeds 80% for 10 consecutive frames, the strategy switching is triggered.

[0103] ③ Multimodal decision fusion. A two-tier decision-making mechanism is used. The primary decision generates candidate trigger instructions based on feature matching results. The secondary verification integrates multimodal data such as IMU trajectory fit and visual matching confidence, and calculates the trigger confidence through a Bayesian network:

[0104]

[0105] in, represents the confidence obtained from the previous visual matching, represents the trajectory fitting error of the inertial measurement unit, so Represents the IMU trajectory fitting reliability. Weight Dynamically adjust according to the environment, such as appropriately increasing the visual weight in good lighting conditions When the IMU moves violently, reduce the weight of the inertial measurement unit appropriately. .

[0106] ④ Dynamic environment adaptive optimization. During the drone's flight, incremental learning is used to update the pre-stored feature library. The feature library is expanded based on the appearance of new landmarks in the sparse semantic map constructed by SLAM, and outdated features are automatically cleared based on access frequency and timeliness, thus achieving real-time updating of the feature library during the decision-making process.

[0107] 2.1 Hierarchical Feature Pyramid (ORB+SIFT Combination)

[0108] ORB is an improvement on FAST corner detection and BRIEF descriptors. It's fast and suitable for real-time applications, but can struggle with scale and rotation variations. SIFT, on the other hand, uses DoG to detect keypoints, is scale- and rotation-invariant, and offers a more robust descriptor, but at the expense of computational complexity. Therefore, combining the two through feature fusion can address the limitations of single feature descriptors in complex scenes.

[0109] The feature pyramid is divided into the bottom layer (L0-L2) and the upper layer (L3-L5). ORB is used to extract edges / corners (geometric primitives) in the bottom layer, and SIFT is used to extract building contours / texture areas (semantic objects) in the upper layer. First, a Gaussian pyramid needs to be generated:

[0110]

[0111] in For the layer images, is the Gaussian kernel weight, usually a 5×5 kernel (k=2).

[0112] ORB (Oriented FAST and Rotated BRIEF) combines FAST corner detection and BRIEF descriptor, and improves it to increase rotation invariance and scale invariance. The FAST corner detection part determines whether the candidate pixel The surrounding 16 pixels have continuous Points of satisfaction ( The threshold is usually 10-30) to achieve corner point screening detection, and further proposed rBRIEF rotation invariant descriptor based on BRIEF descriptor:

[0113]

[0114] in It is an indicator function, which takes 1 when the condition is met and 0 otherwise. By dividing the 256 pairs of sampling points of BRIEF Around keypoint main direction Rotate to get:

[0115]

[0116] In addition, ORB inter-layer association is also added to establish feature correspondence between adjacent layers of the pyramid:

[0117]

[0118] in is the number of pyramid levels, and Hamming distance is used for binary descriptor matching.

[0119] SIFT directly detects key points in the DoG (Difference of Gaussian) pyramid and assigns key point directions. The first step is to calculate the gradient magnitude and direction:

[0120]

[0121] The gradient direction of the key point neighborhood (16×16 window) is divided into 36 bins, and the peak value is taken as the main direction. Then, the key point neighborhood is divided into 16 sub-blocks, and the 8-directional gradient histogram is calculated for each sub-block to finally generate the SIFT descriptor:

[0122]

[0123] By statistically analyzing the cluster distribution of SIFT features in high-level pyramids (such as K-means), semantic categories such as building outlines are marked:

[0124]

[0125] in are pre-trained cluster centers (such as "windows", "roofs", etc.).

[0126] 2.2 Trigger Criteria and Strategy Switching

[0127] We previously described how to construct a hierarchical feature pyramid and the principles and integration of ORB and SIFT. Next, we will describe how to use the previously extracted features to construct triggering criteria for multimodal navigation and switch navigation strategies.

[0128] First, the feature descriptors extracted by ORB and SIFT are used to construct a fusion feature vector. The ORB algorithm at the bottom of the feature pyramid extracts binary descriptors. , characterizes the local geometric structure. The upper layer SIFT generates a 128-dimensional gradient histogram , describing texture and semantic contour. Normalize ORB and SIFT vectors by L2:

[0129]

[0130]

[0131] Fuse the normalized feature vectors:

[0132]

[0133] Secondly, perform cosine similarity calculation and real-time evaluation. Pre-store target area features Generate through offline graph and store as normalized vector. Define current feature With target features The similarity is:

[0134]

[0135] The output range , the closer the value is to 1, the more closely the environment matches the target area.

[0136] Finally, the trigger criteria are generated. The maintenance length is Window , store the similarity of consecutive frames. Set the policy switching trigger condition to all frames in the window that meet :

[0137]

[0138] That is, when continuous When the cosine similarity of the frames is greater than 80%, the navigation strategy is switched and the UAV autonomous navigation system switches from visual inertial navigation to image matching navigation.

[0139] Step 4: Construct image matching navigation link;

[0140] The image matching navigation solution specifically includes the following steps:

[0141] ① Image generation and style transfer. Using the Latent Diffusion model, we convert camera images into stock-style images (removing factors such as season, weather, and perspective). Specifically, during the encoding phase, we use VQ-VAE to compress the image into a latent space. During the diffusion phase, we perform denoising in the latent space to reduce computational complexity.

[0142] ② Real-time feature extraction. Using the XFeat model, the novel strategy of "doubling the network's convolutional depth when the input image resolution is halved" achieves a trade-off between network accuracy and acceleration gains. Specifically, the backbone architecture consists of two parts: the Keypoint Head and the Desriptor Head. The former locates distinctive feature points in the image, while the latter generates high-dimensional feature vectors for these keypoints.

[0143] ③ Image feature matching. Given a dense local feature map , the input of the feature matching link is a subset of 1 / 8 of the spatial resolution From the image pair The nearest neighbor matching of two adjacent matching features , . Predicted pose offset

[0144]

[0145] in is the logarithm of the probability distribution over the possible offsets. Perform classification to achieve correct pixel-level matching:

[0146]

[0147] ④ Posture optimization and map point update. Construct an optimization problem and match the image matching results (corresponding relationship between feature points) ) as input and construct the objective function:

[0148]

[0149] in is the camera pose, are the coordinates of the 3D map point, Represents the camera projection model. Then, the reprojection error is gradually reduced through nonlinear optimization methods. Finally, the sliding window optimization method is used to marginalize the old poses and map points, and the positions of nearby map points are updated through new observations to achieve map point updates.

[0150] 3.1 Image Generation and Style Transfer

[0151] Before matching the features of real-time images with pre-stored image libraries, it is necessary to convert the captured real-time images into photos in the style of pre-stored image libraries to increase the accuracy of the matching. The Latent Diffusion model migrates the diffusion process to a low-dimensional latent space, preserving the quality of the generated image while maintaining efficiency. Therefore, this model is used to perform this operation. The training process of the model is as follows: Figure 5 As shown, the process is divided into two steps:

[0152] First, VQ-VAE is trained. The input image is trained through a deep convolutional network. Compressed into a low-dimensional continuous latent representation ,in ( is the downsampling rate, such as 16), is the potential vector dimension (usually 256 or 512). Introducing a learnable codebook , encoder output Then, quantize to the codebook vector through nearest neighbor search :

[0153]

[0154] in . Then the quantized potential representation As a decoder Input, output reconstructed image .

[0155] Secondly, a diffusion model LDM is trained to learn the noise The generation process of , where the upper part is the noise adding process, used to add noise to the feature The lower part is the denoising process. The core structure is a U-Net composed of Cross Attention, which is used to Restore to The diffusion model can be understood as a temporal denoising autoencoder, whose goal is to exist The objective function of DM (Diffusion Model) can be expressed as follows:

[0156]

[0157] in is in the sequence It is worth noting that the LDM we use is learned in the latent space, that is, the prediction is The noise added above, the corresponding loss function is expressed as:

[0158]

[0159] Example 2

[0160] Embodiment 2 of the present invention provides a long-range and long-duration integrated navigation system that combines visual-inertial joint optimization with image matching, which is implemented based on a monocular camera and an IMU. Figure 2 As shown, the system includes:

[0161] The data acquisition module is used to obtain an offline map of the environment surrounding the UAV flight path by querying satellite information, and preset a number of waypoints in the mission path according to the map conditions;

[0162] The visual inertial navigation module is used to perform high-frequency relative navigation between waypoints using visual inertial navigation to achieve continuous pose estimation; the specific processing process is the same as Step 2 of Example 1;

[0163] The multimodal navigation autonomous decision-making module is used to trigger the switching of the navigation strategy when approaching a waypoint based on the constructed multimodal navigation autonomous decision-making solution, and switch to using the image matching navigation module; the specific processing process is the same as Step 3 of Example 1;

[0164] The image matching navigation module is used to implement image matching navigation based on the real-time photos taken by the camera using the Latent Diffusion model and the XFeat model to correct the relative errors accumulated between waypoints. The specific processing process is the same as Step 4 of Example 1.

[0165] The visual inertial navigation module, multimodal navigation autonomous decision module, and image matching navigation module are reused between multiple waypoints until the mission path is traversed.

[0166] It is worth noting that in the embodiment of the above system, the modules included are only divided according to functional logic, but are not limited to the above division, as long as the corresponding functions can be achieved; in addition, the specific names of the functional modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of the present invention.

[0167] Summary:

[0168] The present invention establishes a combined navigation mechanism of "visual-inertial navigation between waypoints, and multimodal navigation with autonomous decision-making to switch to image matching navigation near waypoints". This corrects the relative error accumulation of visual-inertial navigation between waypoints and avoids the problems of pure image matching methods, such as high deployment cost, poor environmental adaptability, and difficulty in meeting the rapid response requirements of dynamic and complex scenarios. It achieves accurate tracking and positioning of the UAV during flight, thereby improving the navigation and positioning accuracy of the UAV in long-range and long-flight scenarios.

[0169] Finally, it should be noted that the above embodiments are intended only to illustrate the technical solutions of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the embodiments, it should be understood by those skilled in the art that modifications or equivalent substitutions to the technical solutions of the present invention do not depart from the spirit and scope of the technical solutions of the present invention and are intended to be encompassed by the claims of the present invention.

Claims

1. A long-range, long-duration integrated navigation method combining visual-inertial joint optimization and image matching, based on a monocular camera and an IMU, including: Step 1: Obtain an offline map of the drone's flight path and surrounding environment by querying satellite information, and preset several waypoints in the mission path based on the map. Step 2: Use visual inertial navigation for high-frequency relative navigation between waypoints to achieve continuous pose estimation; Step 3: Based on the constructed multimodal navigation autonomous decision-making scheme, the navigation strategy is switched to image matching navigation when approaching a waypoint. Step 4: Based on the real-time photos taken by the monocular camera, the Latent Diffusion model and the XFeat model are used to implement image matching navigation and correct the relative errors accumulated between waypoints. Step 5: Repeat steps 2 to 4 between multiple waypoints until the mission path is traversed; The multimodal navigation autonomous decision in step 3 includes: A hierarchical feature pyramid is used to extract the semantic feature vectors and geometric feature vectors of the environment in the current map, and the extracted feature vectors are normalized and fused; A similarity evaluation algorithm is used to calculate the cosine similarity between the fused feature vector and the pre-stored target area features. When the similarity of a set number of consecutive frames exceeds the threshold, the navigation strategy is switched to the image matching navigation method. The step 4 comprises: Convert real-time photos taken by the camera into gallery-style images through the trained Latent Diffusion model; Use the trained XFeat model to match the style-converted real-time images taken around the waypoints with the offline library photos, predict the pose offset, and achieve pixel-level matching through classification. Calculate the current UAV posture based on the matching results; Using the sliding window optimization method, the old poses and map points are marginalized, and the positions of nearby map points are updated through new observations to achieve map point updates, thereby correcting the accumulated relative errors between waypoints.

2. The long-range and long-duration integrated navigation method combining visual-inertial joint optimization and image matching according to claim 1 is characterized in that: The step 2 includes: Step 2-1: Pre-integrate the IMU data collected by the IMU and construct the pre-integration error term; Step 2-2: For the real-time image captured by the monocular camera, select key frames and process them using the minimum photometric error direct method to obtain the image photometric errors of adjacent key frames; Step 2-3: Perform joint error optimization on the IMU pre-integration error term and the image photometric error to obtain more accurate camera pose information.

3. The long-range and long-duration integrated navigation method combining visual-inertial joint optimization and image matching according to claim 2 is characterized in that: The pre-integrated error term constructed in step 2-1 For the time interval The difference between the predicted value and the actual observed value satisfies the following formula: ； in, yes The logarithmic mapping of is the Lie algebra vector extraction operation, , , They are respectively from Time has come Pre-integrated rotation, position and velocity increments at time, , , , , , , respectively represent the IMU in the world coordinate system Moment and The rotation, position and velocity at each moment, is the gravitational acceleration vector in the global coordinate system, is the time interval, and the superscript T indicates the transpose.

4. The long-range and long-duration integrated navigation method combining visual-inertial joint optimization and image matching according to claim 2 is characterized in that: The step 2-2 includes: Calculate the difference in the gradient magnitude matrix between any two adjacent frames of images; Each frame image is divided into 32×32 grids, and each grid retains the point with the largest difference in gradient amplitude between adjacent frames as the primary image key frame; Dynamically eliminate image key frames with insufficient stability to obtain several key frames; For adjacent keyframes after dynamic culling , the corresponding image photometric error is obtained according to the following formula : ； in, represents the grayscale value of the image, Keyframe The pixel coordinates of Project the pixel to the current keyframe through pose transformation 's coordinates.

5. The long-range and long-duration integrated navigation method combining visual-inertial joint optimization and image matching according to claim 2, characterized in that: The steps 2-3 include: The IMU pre-integration error term and image photometric error are unified into a nonlinear least squares problem. The sliding window optimization method is used to marginalize the old poses and map points. The positions of nearby map points are updated through new observations to obtain more accurate camera pose information.

6. The long-range and long-endurance integrated navigation method combining visual-inertial joint optimization and image matching according to claim 1 is characterized in that: The hierarchical feature pyramid includes a bottom layer and a top layer, wherein: The bottom layer uses ORB to extract geometric features including edges and corners, and the top layer uses SIFT to extract semantic objects including building outlines and texture areas.

7. A system based on the long-range and long-endurance integrated navigation method combining visual-inertial joint optimization and image matching according to claim 1, implemented based on a monocular camera and an IMU, characterized in that: The system includes: a data acquisition module, a visual inertial navigation module, a multimodal navigation autonomous decision module and an image matching navigation module; wherein, The data acquisition module is used to obtain an offline map of the environment surrounding the UAV flight path by querying satellite information, and preset a number of waypoints in the mission path according to the map conditions; The visual inertial navigation module is used to perform high-frequency relative navigation between waypoints using visual inertial navigation to achieve continuous pose estimation; The multimodal navigation autonomous decision-making module is used to trigger the switching of navigation strategies when approaching a waypoint based on the constructed multimodal navigation autonomous decision-making scheme, and switch to using the image matching navigation module; The image matching navigation module is used to implement image matching navigation based on real-time photos taken by the camera using the Latent Diffusion model and the XFeat model to correct the relative errors accumulated between waypoints; The visual inertial navigation module, multimodal navigation autonomous decision module, and image matching navigation module are reused between multiple waypoints until the mission path is traversed.

Images