Remote long-endurance integrated navigation method and system combining visual inertia joint optimization and image matching

Abstract

The invention belongs to the technical field of unmanned aerial vehicle autonomous navigation and positioning, and relates to a remote long-endurance integrated navigation method and system combining visual inertia joint optimization and image matching, and the method comprises the following steps: 1, obtaining an offline map of a surrounding environment of a flight path of an unmanned aerial vehicle by querying satellite information, presetting a plurality of waypoints in the task path according to the map condition; 2, performing high-frequency relative navigation between the waypoints by adopting visual inertial navigation to realize continuous pose estimation; 3, on the basis of the constructed multi-mode navigation autonomous decision scheme, switching of navigation strategies is triggered when a waypoint is approached, and image matching navigation is adopted; step 4, according to a real-time picture shot by the monocular camera, realizing image matching navigation by utilizing a Latent Diffusion model and an XFat model, and correcting relative errors accumulated among waypoints; and step 5, repeating the step 2 to the step 4 among the plurality of waypoints until the task path is traversed.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous navigation and positioning of unmanned aerial vehicles, and particularly relates to a long-range and long-endurance integrated navigation method and system combining visual-inertial joint optimization and image matching. Background Art

[0002] The autonomous navigation and positioning technology of unmanned aerial vehicles determines its own position and attitude through the information observed by sensors in a completely unknown environment, providing a data basis for subsequent tasks such as map construction and navigation. Autonomous positioning is not only a core component of unmanned systems, but also the key to ensuring their stable operation in complex environments.

[0003] Traditional unmanned systems often use the fusion of satellite navigation and inertial navigation. However, the satellite positioning system is greatly affected by the environment. When working in valleys, building clusters, or indoors, satellite signals are easily blocked or multipath effects occur; during wartime, signals are easily blocked or interfered. When satellite positioning fails, pure inertial navigation is easily affected by sensor errors, system noise, and integral cumulative errors. After long-term use, the position and speed information drifts, and additional time is required to correct the accumulated errors, resulting in instability and inaccuracy of the navigation system.

[0004] Pure vision positioning was proposed by Stanford University as early as the 1980s. However, due to the limitations of sensor accuracy and processor performance, it has only been in the experimental research stage for a long time. With the progress of electronic information technology in the 21st century, vision-based positioning has made great progress. Currently, there are two types of methods for using vision for navigation: relative navigation and absolute navigation. Relative navigation is the SLAM technology, which does not rely on a reference map library and can independently solve the relative pose according to the correlation of two frames of images. However, the disadvantage is that there is an error accumulation phenomenon; absolute navigation realizes the positioning of the unmanned aerial vehicle through image matching, and can solve the absolute pose according to the reference map library, without cumulative errors, but it is necessary to store the reference map library in advance, which is difficult to implement for long-endurance unmanned aerial vehicle deployments and requires a large amount of storage.

[0005] For relative navigation (SLAM) technology, the visual / inertial sensor fusion method is commonly used at present. The earliest visual / inertial system was proposed by Anastasios Mourikis et al. from the University of California, Riverside in 2007. Subsequently, a series of classic algorithms such as VI-ORB, VINS-Mono, and ORB-SLAM3 emerged. VI-ORB (Visual-Inertial ORB-SLAM), proposed in 2017, is a visual-inertial fusion system based on the ORB-SLAM framework, which integrates IMU data and ORB visual features through a tightly coupled method. Its core improvement lies in enhancing the trajectory accuracy by jointly optimizing the visual reprojection error and the IMU measurement error. However, this method still has certain limitations: 1) It is sensitive to the time synchronization and calibration errors between the IMU and the camera, and small deviations may lead to cumulative errors; 2) It does not explicitly handle dynamic objects and relies on the static environment assumption, and dynamic targets may interfere with the positioning results. VINS-Mono, proposed in the same year, is a representative framework for monocular visual-inertial SLAM, which adopts a tightly coupled optimization strategy to fuse IMU and monocular image data. Its innovation is to balance accuracy and efficiency through the combination of sliding window optimization and global pose graph adjustment. The main disadvantages include: 1) It does not integrate a dynamic object detection module, and the positioning may fail in dynamic scenarios; 2) The loop closure detection relies on the visual bag-of-words model, and false matches are likely to occur in case of repeated textures or perspective changes. ORB-SLAM3, proposed in 2020, is the third generation of the ORB-SLAM series, supporting multi-modal inputs (monocular, binocular, RGB-D, visual-inertial). Its core improvements are: 1) Introducing a multi-map system (Atlas) to support cross-scene map fusion and long-term positioning; 2) Optimizing the visual-inertial initialization process to improve the convergence speed and robustness; but problems still exist: 1) Limited dynamic environment processing ability, still following the static assumption and not explicitly removing dynamic features; 2) The divergence problem caused by the long-term accumulation of errors in the visual-inertial mode; 3) Insufficient map density, and a practical-level map needs to rely on an extension module to be constructed. From the above analysis, it can be seen that in the context of cross-view, multi-scene, and long-endurance scenarios, there will inevitably be an error accumulation problem in the visual-inertial navigation solution.

[0006] The image matching method for absolute navigation originated from the terminal guidance of cruise missiles and gradually developed into a visual navigation technology later. The scene matching navigation systems (SMNS) have the characteristics of simple device structure, passive mode, and relatively high positioning accuracy. It uses an image sensor to obtain the regional image near the flight or target area and matches it with the stored reference image to obtain the position data of the aircraft. Unmanned aerial vehicle image matching navigation systems such as Figure 1As shown in the figure, after obtaining the destination, preliminary path planning is carried out through satellite images and aerial photos. Subsequently, an adaptation area on the planned flight path is selected to obtain a reference map through calibration. Then, the real-time map obtained by the vision sensor is matched with the reference map to achieve navigation and positioning. Its core is the image matching algorithm, and its performance directly determines the performance of the navigation system. According to different visual feature extraction methods, image matching algorithms can be divided into: template matching-based methods, local invariant feature-based methods, and scene semantic learning-based methods. As one of the representative algorithms of the earliest unmanned aerial vehicle (UAV) vision positioning methods, the UAV scene matching and positioning algorithm based on template matching uses image information such as pixel intensity to construct a matching model to complete scene matching. Compared with the image matching algorithm based on template matching, the image matching algorithm based on local invariant features (also known as handcrafted features) shows stronger environmental adaptability. Local invariant features, that is, handcrafted feature descriptors, usually rely on experts' prior knowledge in algorithm design, and the supplement of this knowledge shows better results than the template matching method in many applications. With the development of computer vision, due to the ability of deep neural networks to capture high-dimensional semantic features in images, scene semantic learning-based methods have emerged. Compared with template matching and local invariant feature methods, the advantage of the image matching method based on scene semantic learning is that the deep neural network has the ability to extract semantic information, so it can better establish image matching relationships. At the same time, with the extraction of semantic information, the image matching algorithm can handle more complex perspective changes and overcome various possible interferences, such as illumination changes, noise, etc. As a representative of the multi-view scene matching algorithm based on metric learning, Zheng et al. publicly released the first multi-source multi-view scene matching UAV vision positioning dataset, University-1652, which provides support for multi-view scene matching research. On this basis, the author constructed a three-branch siamese neural network model to complete scene matching by establishing the corresponding relationship between multi-view UAV images, street view images, and satellite images. The feature extraction model adopted the classic ResNet structure without additional improvement. Yang et al. fully exploited the position encoding of features to solve the perspective difference in the UAV vision positioning algorithm when matching images. Using the position encoding information, the dual-branch siamese network was guided to establish the regional correspondence between multi-view image pairs. With the help of position encoding, the image perspective relationship was marked, and the network was able to learn the regional correspondence association after perspective change, improving the multi-view robustness of the algorithm. However, due to the choice of Transformer as the feature extraction backbone network and the large complexity of the network caused by the spatial position correspondence module, there are certain limitations in both training and mobile platform deployment.Wang et al. proposed the local pattern network, which adopts a square ring feature partitioning strategy to obtain features that describe the central target and the surrounding environment information. By measuring the corresponding feature pairs multiple times, the similarity results based on the central and edge square rings are obtained, and finally the similarity measurement results are obtained through fusion. However, in the process of image region partitioning, the circular partitioning pattern of similarity measurement is completely fixed, which makes it difficult for the algorithm to self-adjust and make optimal decisions in different situations. It is not difficult to see from the above description of the image matching method for scene semantic learning that as a feature matching method with deep learning as the core, this method can handle complex scenes well and has excellent generalization and broad prospects. In contrast, the defect of this method is that it has high requirements for hardware conditions and the offline map dataset of the scene, and it is also impossible to balance model lightweight and matching performance.

[0007] In short, in the autonomous navigation and positioning of unmanned aerial vehicles (UAVs), two core problems are often faced in long-range and long-endurance missions: First, in the environment without GNSS signals, due to the continuous accumulation of errors in visual inertial navigation, the positioning accuracy decreases significantly as the flight range increases; Second, although the traditional image matching-based positioning method can provide absolute position correction, its dependence on high-quality reference maps throughout the process makes it difficult to deploy, has poor real-time performance, and is difficult to work stably when the reference maps are missing or changed in some areas. 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-endurance 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-endurance integrated navigation method combining visual inertial joint optimization and image matching, which is implemented based on a monocular camera and an IMU, and includes:

[0010] Step 1: Obtain an offline map of the surrounding environment of the UAV flight path by querying satellite information, and preset several waypoints in the mission path according to the map situation;

[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 multi-modal navigation autonomous decision-making scheme, trigger the switching of navigation strategies when approaching waypoints and switch to using image matching navigation;

[0013] Step 4: According to the real-time photos taken by the camera, use the Latent Diffusion model and the XFeat model to achieve image matching navigation and correct the relative errors accumulated between waypoints;

[0014] Step 5: Repeat Steps 2 to 4 among multiple waypoints until the task path is traversed.

[0015] Preferably, Step 2 includes:

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

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

[0018] Step 2-3: Joint error optimization is performed 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 the difference between the predicted value and the actual observed value within the time interval and satisfies the following formula:

[0020]

[0021] where is the logarithmic mapping of , is the Lie algebra vector extraction operation, , , are the pre-integration rotation, position, and velocity increments from time to time respectively, , , , , , respectively represent the rotation, position, and velocity of the IMU in the world coordinate system at time and time respectively, is the gravitational acceleration vector in the global coordinate system, is the time interval, and the superscript T is the transpose.

[0022] Preferably, Step 2-2 includes:

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

[0024] Divide each frame of the image into 32×32 grids, and retain the point with the largest difference in gradient magnitude between adjacent frames in each grid as the initially selected image key frame;

[0025] Dynamically eliminate the key frames of images with insufficient stability to obtain several key frames;

[0026] For adjacent key frames after dynamic elimination , obtain the corresponding image photometric error according to the following formula :

[0027]

[0028] where represents the grayscale value of the image, is the pixel coordinate of the key frame , is the coordinate where the pixel is projected onto the current key frame through pose transformation.

[0029] Preferably, the step 2-3 includes:

[0030] Unify the IMU pre-integration error term and the image photometric error into a non-linear least squares problem, adopt a sliding window optimization method, marginalize the old pose and map points, and update the positions of nearby map points through new observations to obtain more accurate camera pose information.

[0031] Preferably, the multi-modal navigation autonomous decision-making in the step 3 includes:

[0032] Adopt a hierarchical feature pyramid to extract the semantic feature vector and geometric feature vector of the environment in the current map, normalize the extracted feature vectors, and perform fusion;

[0033] Adopt a similarity evaluation algorithm to calculate the cosine similarity between the fused feature vector and the pre-stored target area feature. When the similarity of a continuously set number of frames exceeds the threshold, trigger the switching of the navigation strategy and switch to using the image matching navigation method.

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

[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 regions.

[0036] Preferably, the step 4 includes:

[0037] Convert the real-time photo taken by the camera into a picture in the gallery style through a trained Latent Diffusion model;

[0038] Use the trained XFeat model to perform image feature matching between the style-converted real-time image taken around the waypoint and the off-line gallery photo, predict the pose offset, and achieve pixel-level matching through classification.

[0039] Calculate the pose of the current UAV from the matching results;

[0040] Using the sliding window optimization method, marginalize the old pose and map points, update the positions of nearby map points through new observations, and achieve map point update, thereby correcting the accumulated relative error between waypoints.

[0041] On the other hand, the present invention provides a long-range and long-endurance integrated navigation system combining visual-inertial joint optimization and 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 multi-modal navigation autonomous decision-making module, and an image matching navigation module; wherein,

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

[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 multi-modal navigation autonomous decision-making module is used to trigger the switching of the navigation strategy when approaching a waypoint based on the constructed multi-modal navigation autonomous decision-making scheme, and switch to using the image matching navigation module;

[0045] The image matching navigation module is used to perform image matching navigation according to the real-time photos taken by the camera, and use the Latent Diffusion model and the XFeat model to correct the accumulated relative error between waypoints;

[0046] Repeatedly use the visual-inertial navigation module, the multi-modal navigation autonomous decision-making module, and the image matching navigation module between multiple waypoints until the mission path is traversed.

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

[0048] 1. An image matching navigation method based on the Latent Diffusion and XFeat models

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

[0050] 2. Multi-modal navigation autonomous switching strategy

[0051] A strategy for autonomously switching from visual inertial navigation to image matching navigation is constructed. First, hierarchical feature pyramids (a combination of ORB + SIFT) are used to extract the semantic features and geometric features of the current environment. Secondly, according to the similarity evaluation algorithm, the cosine similarity between the environmental features obtained in real time by the current camera and the features of the pre-stored target area is calculated. When the similarity exceeds the threshold, the navigation strategy is triggered to switch, and the image matching navigation method is used instead.

[0052] 3. Integrated navigation scheme based on visual inertial navigation and image matching method

[0053] The present invention proposes an integrated navigation strategy that uses visual inertial navigation between flight path points and image matching navigation near the flight path points. The advantage of this method is that it can effectively compensate for the cumulative relative error caused by the long-term operation of visual inertial navigation. For the image matching navigation algorithm, defining the area near the flight path points as the application scope greatly reduces our requirements for the types of data sets. The distinctiveness of the features around the flight path points also enables us to complete the feature matching task with a lightweight model. These unique advantages exactly complement the highly mature visual inertial navigation, achieving a further improvement in positioning accuracy. Brief Description of the Drawings

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

[0055] Figure 2 is the technical solution framework diagram of the long-range and long-endurance integrated navigation system combining visual inertial joint optimization and image matching of the present invention;

[0056] Figure 3 is the technical solution description diagram 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 It is a framework diagram of a visual inertial navigation solution;

[0058] Figure 5 It is the training process of the Latent Diffusion model. Specific implementation manner

[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 and low-drift pose estimation; when approaching a waypoint, an image matching method is introduced to correct the current navigation state and correct the relative error accumulated between waypoints. In this way, the advantages of the two technologies are effectively integrated, ensuring both the positioning continuity and high precision in the middle section of the voyage, and significantly reducing the dependence on the complete reference map for the entire voyage. This solution improves the adaptability and reliability of the system in complex and non-cooperative environments. Especially when part of the reference map is missing or changed, it can still maintain the continuity and stability of the navigation accuracy, thus 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 implementation of the algorithm framework, including the following steps:

[0061] Step1: Construct a data acquisition link. Obtain a map and preset waypoints. Query satellite information to obtain an offline map of the environment around the UAV flight path, and preset several waypoints in the mission path according to the map situation.

[0062] Step2: Construct a visual inertial navigation link. Between waypoints, the visual inertial navigation method is used. The IMU pre-integration error term and the image photometric error are respectively assigned corresponding weights to form a joint error term, and then all state parameters are jointly optimized by a non-linear optimization method.

[0063] Step3: Construct a multi-modal navigation autonomous decision-making link. Use a hierarchical feature pyramid (ORB+SIFT combination) to extract the semantic features and geometric features of the environment in the pre-stored offline map, and then calculate the cosine similarity between the real-time environment features obtained by the current camera and the pre-stored target area features according to the similarity evaluation algorithm. When the similarity exceeds the threshold, trigger a navigation strategy switch and switch to using the image matching navigation method.

[0064] Step 4: Construct the image matching navigation link. The real-time photos taken by the camera are converted into gallery-style pictures through the Latent Diffusion model, and then XFeat is used to perform feature matching between the real-time images with converted styles taken around the waypoints and the offline gallery photos, and the pose of the current UAV is calculated. Through the above technical means, the relative error generated by visual inertial navigation is corrected, and the positioning accuracy of the UAV in the multi-scene long-endurance navigation task is improved. For the overall structural framework of the technical solution, please refer to Figure 2 , and for the vivid description of the solution, please refer to Figure 3 .

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

[0066] Embodiment 1

[0067] The embodiment of the present invention proposes a long-range long-endurance integrated navigation method combining visual inertial joint optimization and image matching. It includes the following parts:

[0068] Step 1: Construct the data acquisition link;

[0069] Step 2: Construct 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, within a continuous time window, integrate the gyroscope angular velocity and accelerometer data in the local coordinate system 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 a discrete pose constraint to form a pre-integration observation model and construct the pre-integration error term . Finally, transfer the Jacobian matrix and covariance to the backend optimizer to achieve efficient state update.

[0072] ② Visual navigation branch. Process the real-time pictures taken by the camera using the direct method of minimum photometric error. First, it is necessary to ensure the calibration of the camera to determine the internal parameters of the camera (such as focal length, principal point coordinates, distortion coefficients, etc.), so as to ensure the effectiveness of the photometric error in subsequent calculations and the performance of the algorithm. Secondly, extract the key frames of the image. Convolve the captured image with the Sobel / Prewitt operator to calculate the gradient magnitude of each pixel. Divide the image into 32×32 grids, and retain the point with the first gradient magnitude in each grid as the initially selected image key frames. After the initial selection, dynamically eliminate the image key frames with insufficient stability. Finally, calculate the photometric error between adjacent frames .

[0073] ③ Construction and optimization of the combined error term. The photometric error and pre-integration error terms are constructed through the above two branches respectively, and the combined error optimization of the backend is carried out, as Figure 4 shown. The core idea of the combined optimization is to dynamically balance the constraint weights of the two sensors and adjust all state parameters in the non-linear optimization. After the combined optimization, more accurate camera pose information is obtained. Specifically, the sliding window optimization method is adopted to marginalize the old pose and map points, update the positions of the nearby map points through the new observations, and provide more accurate feature constraints for the subsequent frames, thereby further improving the positioning accuracy of the system. Finally, using the optimized pose, the coordinates and covariance matrix of the map points are recalculated.

[0074] 1.1 Construction of the IMU pre-integration error term

[0075] In the time interval , the IMU pre-integration quantities , , , represent the pre-integration rotation, position, and velocity increments from the moment to the moment respectively, and are defined as:

[0076]

[0077] where is the measured angular velocity and acceleration, is the gyroscope and accelerometer biases. is the IMU sampling interval, is the exponential map of the Lie group .

[0078] Bias compensation and error propagation:

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

[0080]

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

[0082]

[0083] Construction of the pre-integration error term:

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

[0085]

[0086] where is Logarithmic mapping, is the Lie algebra vector extraction operation, , , , , , respectively represent the rotation, position, and velocity of the IMU in the world coordinate system at time and time, and is the gravitational acceleration vector in the global coordinate system.

[0087] 1.2 Construct the image photometric error term

[0088] Before calculating the photometric error, key frames need to be selected first. The most representative frames are selected from the video sequence to summarize the video content or mark significant scene changes. The determination of key frames depends on the gradient change intensity of adjacent frames and is achieved through the following steps:

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

[0090]

[0091] where is the image resolution. The larger the difference value, the more significant the scene change. The gradient magnitude of each pixel . Second, divide the image into 32×32 grids, and retain the point with the first gradient magnitude in each grid as the initially selected image key frame. Finally, dynamically remove the image key frames with insufficient stability.

[0092] Assume that the pixel intensities of the same scene point in adjacent key frames remain unchanged. The photometric error is defined as:

[0093]

[0094] where represents the grayscale value of the image, is the pixel coordinate at time, and is the coordinate of the pixel projected onto the current frame

[0095] 1.3 Joint optimization

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

[0097]

[0098] where is a robust kernel function, is the covariance matrix, and X is the optimization variable (pose, velocity, IMU bias, etc.). The sliding window optimization method is adopted to marginalize the old poses and map points, and update the positions of nearby map points through new observations, so as to provide more accurate feature constraints for subsequent frames.

[0099] Step3: Construct the autonomous decision-making link for multi-modal navigation;

[0100] The autonomous decision-making scheme for multi-modal navigation specifically includes the following steps:

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

[0102] ② Feature matching and trigger criterion generation. Real-time feature similarity evaluation is carried out, and the cosine similarity between the current environmental features and the pre-stored target area features is calculated. When the matching degree exceeds 80% for 10 consecutive frames, the strategy switch is triggered.

[0103] ③ Multi-modal decision fusion. A two-layer decision-making mechanism is adopted. The primary decision generates candidate trigger instructions based on the feature matching results, and the secondary verification fuses multi-modal data such as the IMU trajectory fitting degree and the visual matching confidence. The trigger confidence is calculated through the Bayesian network:

[0104]

[0105] Among them, represents the confidence obtained from the previous visual matching, represents the trajectory fitting error of the inertial measurement unit. Therefore, represents the reliability of the IMU trajectory fitting. The weight is dynamically adjusted according to the environment. For example, the visual weight can be appropriately increased in good lighting conditions and the weight of the inertial measurement unit can be appropriately reduced when the IMU moves violently .

[0106] ④ Adaptive optimization for dynamic environment. During the flight of the UAV, incremental learning is used to update the pre-stored feature library. The feature library is expanded according to the appearance of new landmarks in the sparse semantic map constructed by SLAM, and stale features are automatically cleared based on the access frequency and timeliness, so as to realize the real-time update of the feature library during the decision-making process.

[0107] 2.1 Hierarchical feature pyramid (ORB+SIFT combination)

[0108] ORB is an improvement based on FAST corner detection and BRIEF descriptor. It is fast and suitable for real-time applications, but may perform mediocre under scale and rotation changes. SIFT detects key points through DoG, has scale and rotation invariance, and the descriptor is more robust, but the computational cost is relatively high. Therefore, combining the two through feature fusion can address the limitations of a single feature descriptor in complex scenarios.

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

[0110]

[0111] where is the image of the th layer, is the Gaussian kernel weight, and usually a 5×5 kernel (k = 2) is adopted.

[0112] ORB (Oriented FAST and Rotated BRIEF) combines FAST corner detection and BRIEF descriptor, and is improved to increase rotation invariance and scale invariance. In the FAST corner detection part, it determines whether there are continuous points among the 16 pixels around the candidate pixel that satisfy ( is the threshold, usually taking values from 10 - 30) to achieve the screening and detection of corners. Further, an rBRIEF rotation invariant descriptor is proposed based on the BRIEF descriptor:

[0113]

[0114] where is the indicator function, taking 1 when the condition holds and 0 otherwise. By rotating the 256 pairs of sampling points of BRIEF around the main direction of the key point to obtain:

[0115]

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

[0117]

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

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

[0120]

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

[0122]

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

[0124]

[0125] where are the pre-trained clustering centers (such as "window", "roof", etc.).

[0126] 2.2 Trigger Criterion and Strategy Switching

[0127] The previous section described how to construct a hierarchical feature pyramid and the principles and combination methods of ORB and SIFT. Next, it will be introduced how to use the previously extracted features to construct the trigger criterion for multi-modal navigation and switch the navigation strategy.

[0128] First, a fused feature vector is constructed using the feature descriptors extracted by ORB and SIFT. The ORB algorithm at the bottom layer of the feature pyramid extracts binary descriptors , which characterize the local geometric structure. The SIFT at the upper layer generates a 128-dimensional gradient histogram , which describes the texture and semantic contour. The ORB and SIFT vectors are L2-normalized:

[0129]

[0130]

[0131] The normalized feature vectors are fused:

[0132]

[0133] Secondly, cosine similarity calculation and real-time evaluation are performed. The features of the target area are pre-stored Through offline map generation, it is stored as a normalized vector. Define the current feature and the target feature The similarity is as follows:

[0134]

[0135] where the output range , the closer the value is to 1, the higher the degree of match between the environment and the target area.

[0136] Finally, generate the trigger criterion. Maintain a window of length and store the similarity of consecutive frames. Set the trigger condition for policy switching to that all frames within the window satisfy : That is, when the cosine similarity of consecutive

[0137]

[0138] frames is greater than 80%, switch the navigation policy, and the UAV autonomous navigation system will switch from visual inertial navigation to image matching navigation.

[0139] Step4: Construct the image matching navigation link;

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

[0141] ① Image generation and style conversion. Use the Latent Diffusion model to convert the pictures taken by the camera into pictures in the style of the picture library (removing factors such as season, weather, perspective, etc.). Specifically, in the encoding stage, use VQ-VAE to compress the image into the latent space, and in the diffusion stage, perform the denoising process in the latent space to reduce the computational complexity.

[0142] ② Real-time feature extraction. Adopt the XFeat model, and through the new strategy "when the input image resolution is halved, double the convolutional depth of the network", the model makes a trade-off between network accuracy and acceleration gain. Specifically, the backbone structure consists of two parts: Keypoint Head and Desriptor Head. The former is responsible for locating the significantly discriminative feature points in the image, while the latter generates high-dimensional feature vectors for the key points.

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

[0144]

[0145] where is the logarithm of the probability distribution over the possible offsets. Classify the offset to achieve correct pixel-level matching:

[0146]

[0147] ④ Pose optimization and map point update. Construct an optimization problem, taking the image matching result (feature point correspondence ) as the input, and construct an objective function:

[0148]

[0149] where is the camera pose, are the 3D map point coordinates, represents the camera projection model. Then, gradually reduce the reprojection error through a non-linear optimization method to achieve pose optimization. Finally, use the sliding window optimization method to marginalize the old poses and map points, and update the positions of the nearby map points through new observations to achieve map point update.

[0150] 3.1 Image Generation and Style Transfer

[0151] Before performing feature matching between the real-time image and the pre-stored image library, it is necessary to convert the captured real-time image into a photo in the style of the pre-stored image library to improve the matching accuracy. The Latent Diffusion model migrates the diffusion process to the low-dimensional latent space, retaining the quality of the generated image while being efficient, so this model is used for this operation. The training process of the model is as Figure 5 shown, and this process is divided into two steps:

[0152] First, train the VQ-VAE. Compress the input image into a low-dimensional continuous latent representation , where ( is the downsampling rate, such as 16), is the latent vector dimension (usually 256 or 512). Introduce a learnable codebook , after the encoder outputs , quantize it to the codebook vector through nearest neighbor search:

[0153]

[0154] Among them Then, the quantized latent representation is used as the input of the decoder to output the reconstructed image .

[0155] Secondly, train a diffusion model LDM to learn the generation process from noise to . The upper part is the noise addition process, which is used to add noise to the features to . 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. Its goal is to predict the noise added to the input image at the moment. The objective function of DM (Diffusion Model) can be expressed as the following formula:

[0156]

[0157] Among them is the uniform sampling on the sequence . It should be noted that the LDM we used is learned in the latent space, that is, predicting the noise added to . The corresponding loss function is expressed as:

[0158]

[0159] Embodiment 2

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

[0161] The data acquisition module is used to obtain the offline map of the environment around the UAV flight path by querying satellite information, and preset several waypoints in the mission path according to the map situation;

[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 Step2 of Embodiment 1;

[0163] The multi-modal navigation autonomous decision-making module is used to trigger the switching of the navigation strategy based on the constructed multi-modal navigation autonomous decision-making scheme, and switch to using the image matching navigation module when approaching the waypoint; the specific processing process is the same as Step3 of Embodiment 1;

[0164] The described 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, and correct the accumulated relative error between waypoints; the specific processing process is the same as Step4 in Embodiment 1.

[0165] The visual inertial navigation module, the multi-modal navigation autonomous decision-making module, and the image matching navigation module are repeatedly used among multiple waypoints until the task path is traversed.

[0166] It should be noted that in the embodiments of the above system, the various 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 mutual distinction and do not limit the protection scope of the present invention.

[0167] Summary:

[0168] The present invention establishes a combined navigation mechanism of "visual inertial navigation between waypoints, and multi-modal navigation autonomous decision-making switches to image matching navigation near waypoints", corrects the accumulation of relative errors in visual inertial navigation between waypoints, and also avoids the problems of high deployment cost, poor environmental adaptability, and difficulty in meeting the rapid response requirements of dynamic complex scenarios in the pure image matching method, realizes accurate tracking and positioning of the UAV flight process, and thus improves the navigation and positioning accuracy of the UAV in the long-range and long-endurance scenario.

[0169] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the embodiments, those of ordinary skill in the art should understand that any modification or equivalent replacement of the technical solutions of the present invention does not depart from the spirit and scope of the technical solutions of the present invention, and they should all be covered by the scope of the claims of the present invention.

Claims

1. A long-range and long-endurance integrated navigation method combining visual-inertial joint optimization and image matching, which is implemented based on a monocular camera and an IMU, includes: Step 1: Obtain an offline map of the environment around the UAV flight path by querying satellite information, and preset several waypoints in the mission path according to the map situation; Step 2: Use visual-inertial navigation for high-frequency relative navigation between waypoints to achieve continuous pose estimation; Step 3: Based on the constructed multi-modal navigation autonomous decision-making scheme, trigger the switching of the navigation strategy when approaching a waypoint, and switch to using image matching navigation; Step 4: According to the real-time photos taken by the monocular camera, use the Latent Diffusion model and the XFeat model to achieve image matching navigation, and correct the accumulated relative error between waypoints; Step 5: Repeat Step 2 to Step 4 between multiple waypoints until the mission path is traversed.

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

3. The long-range long-endurance integrated navigation method combining visual-inertial joint optimization and image matching according to claim 2, wherein The pre-integration error term constructed in the step 2-1 is the difference between the predicted value and the actual observed value within the time interval , and satisfies the following formula: ； Among them, is the logarithmic mapping of the Lie algebra vector extraction operation, , , are respectively from the time to the time, the pre-integrated rotation, position and velocity increments, , , , , , respectively represent the rotation, position and velocity of the IMU in the world coordinate system at the time and the time, is the gravitational acceleration vector in the global coordinate system, is the time interval, and the superscript T is the transpose.

4. The long-range long-endurance integrated navigation method combining visual-inertial joint optimization and image matching according to claim 2, characterized in that The said Step 2-2 includes: Calculate the difference in gradient magnitude matrices between any two adjacent frames of images; Divide each frame of image into 32×32 grids, and retain the points with the largest gradient magnitude difference between adjacent frames in each grid as the initially selected image key frames; Dynamically eliminate the image key frames with insufficient stability to obtain several key frames; For adjacent key frames after dynamic culling , the corresponding image photometric error is obtained according to the following formula : ； Among them, represents the grayscale value of the image, is the key frame of the pixel coordinates, is the coordinate where the pixel is projected onto the current key frame by pose transformation.

5. The long-range long-endurance integrated navigation method combining visual-inertial joint optimization and image matching according to claim 2, characterized in that The said Step 2-3 includes: Unify the IMU pre-integration error term and the image photometric error into a non-linear least squares problem, adopt a sliding window optimization method, marginalize the old poses and map points, and update the positions of nearby map points through new observations to obtain more accurate camera pose information.

6. The long-range long-endurance integrated navigation method combining visual-inertial joint optimization and image matching according to claim 1, characterized in that The multi-modal navigation autonomous decision-making in the said Step 3 includes: Use a hierarchical feature pyramid to extract the semantic feature vector and geometric feature vector of the environment in the current map, normalize the extracted feature vectors, and perform fusion; Adopt a similarity evaluation algorithm to calculate the cosine similarity between the fused feature vector and the pre-stored target area feature. When the similarity of a continuously set number of frames exceeds the threshold, trigger the switching of the navigation strategy and switch to using the image matching navigation method.

7. The long-range and long-endurance integrated navigation method combining visual-inertial joint optimization and image matching according to claim 6, characterized in that The said hierarchical feature pyramid includes a bottom layer and a top layer, where 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 regions.

8. The long-range long-endurance integrated navigation method combining visual-inertial joint optimization and image matching according to claim 1, wherein The said Step 4 includes: Convert the real-time photos taken by the camera into gallery-style pictures through the trained Latent Diffusion model; Use the trained XFeat model to perform image feature matching between the style-converted real-time images taken around the waypoint and the offline gallery photos, predict the pose offset, and achieve pixel-level matching through classification; Calculate the pose of the current UAV according to the matching result; Using the sliding window optimization method, marginalize the old poses and map points, update the positions of nearby map points through new observations, and achieve map point update, thereby correcting the accumulated relative error between waypoints.

9. A long-range and long-endurance integrated navigation system that combines visual-inertial joint optimization and image matching, which is implemented based on a monocular camera and an IMU, and is characterized in that The system includes: a data acquisition module, a visual inertial navigation module, a multi-modal navigation autonomous decision-making module, and an image matching navigation module; among them, The data acquisition module is used to obtain an offline map of the environment around the UAV flight path by querying satellite information, and preset several waypoints in the mission path according to the map situation; 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 multi-modal navigation autonomous decision-making module is used to trigger the switching of navigation strategies when approaching waypoints based on the constructed multi-modal navigation autonomous decision-making scheme, and switch to using the image matching navigation module; The image matching navigation module is used to use the Latent Diffusion model and the XFeat model to achieve image matching navigation according to the real-time photos taken by the camera, and correct the accumulated relative error between waypoints; Repeat the use of the visual inertial navigation module, the multi-modal navigation autonomous decision-making module, and the image matching navigation module between multiple waypoints until the mission path is traversed.

Images