Satellite denial high altitude hovering positioning method based on visual perception and inertial measurement
By employing a tightly coupled approach of visual perception and inertial measurement, the problems of low positioning accuracy and low frequency of UAVs in satellite-denied environments were solved, enabling high-frequency, high-precision UAV hovering and enhancing the robustness and stability of the system.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- ZHUOYI ZHINENG
- Filing Date
- 2025-10-30
- Publication Date
- 2026-06-09
AI Technical Summary
Existing UAV positioning systems rely on GNSS signals in satellite-denied environments, which are susceptible to obstruction or spoofing, leading to positioning failure. Pure inertial navigation accumulates and diverges errors. Monocular vision lacks scale information, while binocular vision increases depth estimation errors. The ORB-SLAM algorithm has low initial pose accuracy at high altitudes, and keyframe management does not consider time decay and spatial correlation, resulting in low pose output frequency.
A method based on visual perception and inertial measurement is adopted. Through ORB feature detection, bag-of-words model loop closure detection, PNP algorithm initial pose estimation, sliding window optimization and LM optimization algorithm, combined with IMU pre-integration and extended Kalman filtering, a tightly coupled visual and inertial data fusion is achieved. An adaptive switching mechanism for positioning mode is designed to control the keyframe capacity, remove mismatched feature points, and construct multi-source constrained residual optimized pose.
It achieves high-frequency, high-precision, and high-robust hovering of UAVs in satellite-denied environments, solving the problems of low positioning accuracy and low frequency, and enhancing the stability and reliability of the system in complex environments.
Smart Images
Figure CN121026103B_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of UAV positioning and navigation technology, and in particular to a satellite-denied high-altitude hovering positioning method based on visual perception and inertial measurement. Background Technology
[0002] Tethered unmanned aerial vehicles (UAVs) have significant application value in fields such as communication relay and environmental monitoring due to their continuous loitering capability. Their stable operating altitude is typically 100-300 meters, and achieving precise hovering at this altitude is crucial for ensuring mission effectiveness. Traditional positioning methods heavily rely on GNSS signals, but in satellite-denied environments (such as cities, canyons, and areas with electromagnetic interference), GNSS signals are easily blocked or spoofed, leading to positioning failures and rendering the UAVs unable to function properly. Therefore, under satellite-denied conditions, Simultaneous Localization and Mapping (SLAM) technology is needed to provide accurate positioning information for UAVs.
[0003] In existing technologies, pure inertial navigation suffers from the problem of cumulative error diverging over time; consumer-grade inertial navigation systems can drift by tens of meters in just one hour of operation. Monocular vision positioning lacks scale information and is prone to losing tracking in feature-scarce environments. Binocular vision, in high-altitude scenarios, suffers from baseline limitations, resulting in a dramatic increase in depth estimation error as the UAV's flight altitude increases. LiDAR solutions are limited by detection range and adaptability to adverse weather conditions. Furthermore, existing technologies typically employ the ORB-SLAM algorithm for UAV localization under denied conditions, but this method suffers from several drawbacks in high-altitude down-view scenarios. It lacks an initialization mechanism for ground features, and pure rotational ambiguity affects initial pose accuracy. It also lacks an altitude-based camera mode switching strategy, making it difficult to balance high- and low-altitude positioning performance. Keyframe management does not consider time decay and spatial correlation, leading to excessive computational load during long-term hovering and low system pose output frame rates, thus failing to provide high-frequency and reliable pose data for UAV devices. Summary of the Invention
[0004] In view of this, the purpose of this application is to provide a satellite-rejected high-altitude hovering positioning method based on visual perception and inertial measurement, which can at least solve the problems of low positioning accuracy, easy positioning failure in degraded environments, and low pose output frequency of single sensor positioning systems, and effectively realize a UAV high-altitude hovering method that does not rely on GNSS signals and has high frequency, high accuracy and high robustness.
[0005] This application provides a satellite rejection high-altitude hovering positioning method based on visual perception and inertial measurement, applied to an unmanned aerial vehicle (UAV) control system. The UAV control system includes a front-end processing module and a back-end processing module. The front-end processing module includes a pre-built pose constraint model, and the back-end optimization module is used for precise positioning.
[0006] The front-end processing module performs the following steps: ORB feature detection and extraction on the ground environment image captured by the downward-looking binocular camera to obtain the spatial coordinate information of the feature points, and scale-invariant feature matching is achieved by combining the ground environment image pyramid; a keyframe dictionary is constructed based on the bag-of-words model, and loop closure detection is completed by calculating the similarity of the feature points; initial pose estimation is achieved based on the PNP algorithm and IMU pre-integration to generate an initial estimated pose sequence; and the binocular camera visual data and IMU inertial measurement data are fused using a tight coupling method to construct motion equations and observation equations.
[0007] The backend optimization module performs the following steps: it uses a sliding window optimization algorithm to optimize the initial estimated pose sequence in real time to eliminate accumulated errors; and it uses the LM optimization algorithm to solve for the maximum a posteriori probability estimate to determine the target's high-altitude hovering positioning pose.
[0008] Optionally, the method further includes: performing binocular vision initialization in a static state;
[0009] The steps for initializing binocular vision in a static state include:
[0010] An initial ground feature map is constructed using binocular parallax triangulation.
[0011] The gravity vector is estimated using acceleration data from the IMU when it is stationary, and the rotational component of the camera attitude is constrained to eliminate ambiguity of pure rotation.
[0012] Cross-correlation analysis was used to calibrate the time offset between the image and the IMU data.
[0013] Optionally, after feature matching, the front-end processing module performs the following steps:
[0014] The PNP algorithm is used to estimate the initial pose of the camera, and the Random Sample Consensus (RANSAC) algorithm is used to remove mismatched feature point pairs.
[0015] Construct pose optimization equations using IMU pre-integration data;
[0016] Iterative optimization of poses in adjacent frames is performed to output inter-frame pose transformation matrix and generate initial estimated pose sequence;
[0017] The pose optimization equation is as follows:
[0018] ;
[0019] in, Let be the pose matrix to be determined. For image feature points, For 3D map points, For projection function, For IMU pre-integration pose, For weight fusion.
[0020] Optionally, the orientation of the feature point can be calculated using the following formula:
[0021] Define the image block moments as: ;
[0022] in, , For pixel coordinates, Grayscale value;
[0023] The orientation angle of the feature point is calculated using the following formula:
[0024] ;
[0025] in, and These represent the gray-level weighted sums of the image patch in the image height and width directions, respectively, to achieve rotation-invariant matching of feature points.
[0026] Optionally, the equation of motion is:
[0027] ;
[0028] in, Let k be the state vector at time k. For IMU measurements, This is process noise;
[0029] The observation equation is as follows:
[0030] ;
[0031] in, These are visual feature observations. To observe noise;
[0032] The method further includes: using extended Kalman filtering to update the state vector estimation, wherein the state vector includes position, velocity, attitude, sensor bias, and gravity vector.
[0033] Optionally, the loop closure detection employs a two-layer verification mechanism, specifically including:
[0034] Keyframe similarity is calculated using BoW vectors, and it is determined whether the keyframe similarity is greater than a first preset threshold.
[0035] If the keyframe similarity is greater than the first preset threshold, then the RANSAC algorithm is used to remove mismatched feature points, the fundamental matrix is calculated to verify spatial geometric consistency, and the loop relationship is confirmed.
[0036] Optionally, the backend optimization module specifically performs the following steps:
[0037] Calculate the visual reprojection residual, IMU pre-integration residual, and loop closure residual, and calculate the total residual value based on the pre-established objective function;
[0038] The pose parameters are iteratively optimized using the LM algorithm, and the solution is accelerated by decomposing the sparse matrix using the Schur complement, until the total residual converges to a preset second threshold.
[0039] The visual reprojection residual is calculated using the following formula: ;
[0040] The IMU pre-integration residual is calculated using the following formula. ;
[0041] Calculate the closure residual using the following formula. ;
[0042] in, Let be the pixel coordinates of the i-th 2D image observation point. Let be the camera pose of the j-th frame, represented by the homogeneous transformation matrix. Let represent the camera pose in the k-th frame, expressed as a homogeneous transformation matrix. The 3D spatial coordinates of the i-th 2D point The estimated relative pose of the IMU from frame j to frame k. For camera projection function, The pose of the j-th frame obtained by loop closure matching;
[0043] The objective function is: ;
[0044] in, For pose parameter vectors, The damping factor, For regularization matrix, , , This is the information matrix corresponding to the residual.
[0045] Optionally, the sliding window optimization algorithm sets the maximum keyframe capacity of the window to 500 frames. When the number of keyframes reaches the maximum value, keyframes are filtered by summing the time decay score, isolation score and pose difference score of the candidate keyframes to be deleted, and the keyframe with the lowest score is deleted.
[0046] The time decay score is calculated using the following formula: ;
[0047] in, The difference between the IDs of the newly added keyframe and the candidate deleted keyframe;
[0048] The isolation score is calculated using the following formula: ;
[0049] in, The number of shared map points for newly added keyframes and candidate deleted keyframes;
[0050] The pose difference score is calculated using the following formula:
[0051] ;
[0052] in, The pose difference between the newly added keyframe and the candidate deleted keyframe.
[0053] Optionally, the method further includes: implementing a positioning mode adaptive switching mechanism;
[0054] The adaptive switching mechanism for positioning modes performs the following steps:
[0055] When the drone flies below the altitude threshold, the binocular camera and IMU fusion positioning mode is activated.
[0056] When the drone's flight altitude is higher than or equal to the altitude threshold, it switches to a monocular camera and IMU fusion positioning mode. During the switching process from the binocular camera and IMU fusion positioning mode to the monocular camera and IMU fusion positioning mode, a smooth transition algorithm is used to maintain positioning continuity.
[0057] Optionally, the method further includes:
[0058] When the number of feature points detected is lower than the third threshold or the number of point pairs matched with the map tracking in the current frame is lower than the preset threshold, the pure IMU positioning mode is activated, and the IMU drift is suppressed by the zero-speed correction ZUPT algorithm until the visual features are restored and then switched back to the fusion mode.
[0059] The satellite rejection high-altitude hovering positioning method provided in this application embodiment can at least solve the problems of low positioning accuracy, easy positioning failure in degraded environments, and low pose output frequency of single sensor positioning systems. It can effectively realize a UAV high-altitude hovering method that does not rely on GNSS signals and has high frequency, high accuracy and high robustness.
[0060] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description
[0061] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.
[0062] Figure 1 A flowchart illustrating a satellite rejection high-altitude hovering positioning method based on visual perception and inertial measurement, provided for an embodiment of the present invention;
[0063] Figure 2 This is a flowchart illustrating another satellite rejection high-altitude hovering positioning method based on visual perception and inertial measurement, provided as an embodiment of the present invention. Detailed Implementation
[0064] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.
[0065] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.
[0066] First, the applicable scenarios for this application will be introduced. This application can be applied to the field of unmanned aerial vehicle (UAV) positioning and navigation technology.
[0067] Tethered unmanned aerial vehicles (UAVs) have significant application value in fields such as communication relay and environmental monitoring due to their continuous loitering capability. Their stable operating altitude is typically 100-300 meters, and achieving precise hovering at this altitude is crucial for ensuring mission effectiveness. Traditional positioning methods heavily rely on GNSS signals, but in satellite-denied environments (such as cities, canyons, and areas with electromagnetic interference), GNSS signals are easily blocked or spoofed, leading to positioning failures and rendering the UAVs unable to function properly. Therefore, under satellite-denied conditions, Simultaneous Localization and Mapping (SLAM) technology is needed to provide accurate positioning information for UAVs.
[0068] In existing solutions, pure inertial navigation suffers from the problem of cumulative error diverging over time; consumer-grade inertial navigation systems can drift by tens of meters in just one hour of operation. Monocular vision positioning lacks scale information and is prone to losing tracking in feature-scarce environments. Binocular vision, in high-altitude scenarios, suffers from baseline limitations, resulting in a dramatic increase in depth estimation error as the UAV's flight altitude increases. LiDAR solutions are limited by detection range and adaptability to adverse weather conditions. The industry typically uses the ORB-SLAM algorithm to achieve UAV localization under denied conditions, but this method has the following limitations in high-altitude down-view scenarios: 1. It lacks an initialization mechanism for ground features, and pure rotational ambiguity affects the initial pose accuracy; 2. It lacks an altitude-based camera mode switching strategy, making it difficult to balance high and low altitude positioning performance; 3. Keyframe management does not consider time decay and spatial correlation, resulting in excessive computational load during long-term hovering, low system pose output frame rate, and inability to provide high-frequency and reliable pose data for UAV devices.
[0069] The satellite-rejected high-altitude hovering positioning method proposed in this application can at least solve the problems of low positioning accuracy, easy positioning failure in degraded environments, and low pose output frequency of single sensor positioning systems. It can effectively realize a UAV high-altitude hovering method that does not rely on GNSS signals and has high frequency, high accuracy and high robustness.
[0070] Please see Figure 1 , Figure 1 This is a flowchart illustrating a satellite-denied high-altitude hovering positioning method based on visual perception and inertial measurement, provided as an embodiment of this application. The satellite-denied high-altitude hovering positioning method based on visual perception and inertial measurement proposed in this application is applied to an unmanned aerial vehicle (UAV) control system. The UAV control system includes a front-end processing module and a back-end processing module. The front-end processing module includes a pre-built pose constraint model, and the back-end optimization module is used for precise positioning.
[0071] like Figure 1 As shown in the embodiments of this application, the satellite rejection high-altitude hovering positioning method based on visual perception and inertial measurement includes:
[0072] S101. Perform ORB feature detection and extraction on the ground environment image captured by the downward-looking binocular camera to obtain the spatial coordinate information of the feature points, and combine it with the ground environment image pyramid to achieve scale-invariant feature matching.
[0073] S102. Construct a keyframe dictionary based on the bag-of-words model, and complete loop closure detection by calculating the similarity of the feature points.
[0074] S103. Initial pose estimation is achieved based on the PNP algorithm and IMU pre-integration, generating an initial estimated pose sequence.
[0075] S104. Use a tight coupling method to fuse binocular camera visual data and IMU inertial measurement data to construct motion equations and observation equations.
[0076] S105. The initial estimated pose sequence is optimized in real time using a sliding window optimization algorithm to eliminate accumulated errors.
[0077] S106. Solve the maximum a posteriori probability estimate by using the LM optimization algorithm to determine the target's high-altitude hovering and positioning pose.
[0078] Steps S101-S104 are executed by the front-end processing module, while steps S105 and S106 are executed by the back-end optimization module.
[0079] Optionally, the method further includes performing binocular vision initialization in a static state.
[0080] The steps for initializing binocular vision in a static state include:
[0081] An initial ground feature map is constructed using binocular parallax triangulation.
[0082] The gravity vector is estimated using acceleration data from the IMU when it is stationary, and the rotational component of the camera attitude is constrained to eliminate ambiguity of pure rotation.
[0083] Cross-correlation analysis was used to calibrate the time offset between the image and the IMU data.
[0084] Specifically, after feature matching, the front-end processing module performs the following steps:
[0085] The PNP algorithm is used to estimate the initial pose of the camera, and the Random Sample Consensus (RANSAC) algorithm is used to remove mismatched feature point pairs.
[0086] Construct pose optimization equations using IMU pre-integration data;
[0087] Iterative optimization of poses in adjacent frames is performed to output inter-frame pose transformation matrix and generate initial estimated pose sequence;
[0088] The pose optimization equation is as follows:
[0089] ;
[0090] in, Let be the pose matrix to be determined. For image feature points, For 3D map points, For projection function, For IMU pre-integration pose, For weight fusion.
[0091] The orientation of the feature point can be calculated using the following formula:
[0092] Define the image block moments as: ;
[0093] in, , For pixel coordinates, Grayscale value;
[0094] The orientation angle of the feature point can be calculated using the following formula:
[0095] ;
[0096] in, and These represent the gray-level weighted sums of the image patch in the image height and width directions, respectively, to achieve rotation-invariant matching of feature points.
[0097] Specifically, the equation of motion can be:
[0098] ;
[0099] in, Let k be the state vector at time k. For IMU measurements, This is process noise;
[0100] The observation equation can be:
[0101] ;
[0102] in, These are visual feature observations. To observe noise;
[0103] The method further includes: using extended Kalman filtering to update the state vector estimation, wherein the state vector includes position, velocity, attitude, sensor bias, and gravity vector.
[0104] In some embodiments, the loop closure detection further employs a two-layer verification mechanism, specifically including:
[0105] Keyframe similarity is calculated using BoW vectors, and it is determined whether the keyframe similarity is greater than a first preset threshold.
[0106] If the keyframe similarity is greater than the first preset threshold, then the RANSAC algorithm is used to remove mismatched feature points, the fundamental matrix is calculated to verify spatial geometric consistency, and the loop relationship is confirmed.
[0107] Specifically, the backend optimization module performs the following steps:
[0108] Calculate the visual reprojection residual, IMU pre-integration residual, and loop closure residual, and calculate the total residual value based on the pre-established objective function;
[0109] The pose parameters are iteratively optimized using the LM algorithm, and the solution is accelerated by decomposing the sparse matrix using the Schur complement, until the total residual converges to a preset second threshold.
[0110] The visual reprojection residual is calculated using the following formula: ;
[0111] The IMU pre-integration residual is calculated using the following formula. ;
[0112] Calculate the closure residual using the following formula. ;
[0113] in, Let be the pixel coordinates of the i-th 2D image observation point. Let be the camera pose of the j-th frame, represented by the homogeneous transformation matrix. Let represent the camera pose in the k-th frame, expressed as a homogeneous transformation matrix. The 3D spatial coordinates of the i-th 2D point The estimated relative pose of the IMU from frame j to frame k. For camera projection function, The pose of the j-th frame obtained by loop closure matching;
[0114] The objective function is: ;
[0115] in, For pose parameter vectors, The damping factor, For regularization matrix, , , This is the information matrix corresponding to the residual.
[0116] In some embodiments, the sliding window optimization algorithm sets the maximum keyframe capacity of the window to 500 frames. When the number of keyframes reaches the maximum value, keyframes are filtered by summing the time decay score, isolation score, and pose difference score of the candidate keyframes to be deleted, and the keyframe with the lowest score is deleted.
[0117] The time decay score is calculated using the following formula: ;
[0118] in, The difference between the IDs of the newly added keyframe and the candidate deleted keyframe;
[0119] The isolation score is calculated using the following formula: ;
[0120] in, The number of shared map points for newly added keyframes and candidate deleted keyframes;
[0121] The pose difference score is calculated using the following formula:
[0122] ;
[0123] in, The pose difference between the newly added keyframe and the candidate deleted keyframe.
[0124] Optionally, the method further includes: implementing a positioning mode adaptive switching mechanism;
[0125] The adaptive switching mechanism for positioning modes performs the following steps:
[0126] When the drone flies below the altitude threshold, the binocular camera and IMU fusion positioning mode is activated.
[0127] When the drone's flight altitude is higher than or equal to the altitude threshold, it switches to a monocular camera and IMU fusion positioning mode. During the switching process from the binocular camera and IMU fusion positioning mode to the monocular camera and IMU fusion positioning mode, a smooth transition algorithm is used to maintain positioning continuity.
[0128] Optionally, the method further includes: when the number of feature points detected is lower than a third threshold or the number of point pairs matched with the map tracking in the current frame is lower than a preset threshold, a pure IMU positioning mode is activated, and the IMU drift is suppressed by the Zero-speed correction (ZUPT) algorithm until the visual features are recovered and then switched back to the fusion mode.
[0129] As an example, the satellite rejection high-altitude hovering positioning method based on visual perception and inertial measurement employs a binocular camera and an IMU inertial measurement unit, and is based on a sliding window optimization method, including a pose estimation front-end processing that fuses visual perception and inertial measurement, and a back-end processing that tightly couples keyframe pose and map point optimization.
[0130] For example, the front-end processing module includes steps such as: visual feature extraction; binocular vision initialization; mode adaptive switching; front-end pose estimation; and visual-inertial fusion.
[0131] The visual feature extraction steps include: acquiring ground environment images using a downward-looking binocular camera, extracting ORB feature points from the images, and constructing a multi-layer image pyramid to achieve scale invariance. For each feature point, the orientation angle is calculated using the gray-scale centroid method to ensure rotational invariance matching of the features.
[0132] The binocular vision initialization steps include: when the UAV is stationary, acquiring ground images through a downward-looking binocular camera; triangulating the extracted feature points from the images based on the left-right parallax principle to generate an initial feature map; and utilizing acceleration data from the stationary IMU. We estimate the gravity vector and construct attitude rotation constraint equations to eliminate ambiguity in pure rotation; we calculate the time offset between the image and IMU data using a cross-correlation algorithm. To achieve time alignment of sensor data;
[0133] The steps of adaptive mode switching include: due to the scale uncertainty of monocular vision, the scale recovery accuracy of IMU is not high and the initialization has high requirements for system motion. It usually needs to perform full displacement and rotation motion in six degrees of freedom, which has poor robustness.
[0134] Furthermore, due to the installation position limitations of the downward-looking camera, the field of view is small at low altitudes, resulting in a limited number of highly saliency visual features extracted. This increases the uncertainty in the scale recovery accuracy of monocular vision. In contrast, binocular vision can obtain the spatial physical coordinates of corresponding pixels through the parallax of the left and right cameras, and the depth recovery accuracy of pixels is higher at altitudes below 10m. However, due to the limitation of the baseline length of the binocular camera, the depth recovery accuracy of binocular cameras for distant features is limited. As the system altitude increases, the depth calculation error also gradually increases, which is not conducive to high-altitude positioning. Therefore, the system continuously updates the current positioning pose, and switches to monocular + IMU mode when the system altitude is not less than 10m.
[0135] The front-end pose estimation steps include: performing feature matching between 3D map points and feature points in the current frame image; using the Perspective-n-Point (PNP) algorithm combined with Random Sample Consensus (RANSAC) to eliminate mismatches; and estimating the initial camera pose. Simultaneously acquire IMU pre-integration data and calculate IMU predicted pose. Construct the fusion optimization equation: ,in Dynamically adjusts according to the number of visual features (when features are sufficient) =0.1, when features are scarce =0.5); iteratively solve the pose of adjacent frames and output the inter-frame pose transformation matrix. Accumulate and generate the initial pose sequence .
[0136] The steps of visual-inertial fusion include: using a tightly coupled architecture to construct a state vector containing position, velocity, attitude, sensor bias, and gravity vector from IMU data and visual data, and achieving state prediction and update through extended Kalman filtering.
[0137] For example, the backend processing steps include: constructing multi-source constraint residuals; constructing an objective function; iteratively optimizing using the LM algorithm; and limiting the capacity of keyframes.
[0138] The steps for constructing multi-source constrained residuals include: visual reprojection residuals, which originate from the difference between the observed values of visual feature points on the image plane and the theoretical values of projecting 3D map points onto the image plane through the current pose estimation, for each pair of feature points. Corresponding 3D map points The formula for calculating the residual is: ,in The camera projection function considers the camera's intrinsic and extrinsic parameters, as well as the distortion model, to ensure the accuracy of the projection calculation. The IMU pre-integration residual reflects the consistency between the motion increment obtained through IMU pre-integration and the inter-frame pose transformation based on the current pose estimation between adjacent keyframes. Its residual expression is as follows: ,in For the IMU pre-integration pose transformation from keyframe j to k, and These are the pose matrices for keyframes j and k, respectively. The IMU pre-integration process utilizes the median integration method, based on the acceleration and angular velocity data measured by the IMU, to accurately calculate the motion increment between adjacent image frames, providing important inertial constraints for pose optimization. The loop closure detection residual is used to constrain the pose consistency between loop closure keyframes. When a loop closure is detected, the pose of the loop closure keyframe is calculated. Compared with the current estimated pose The difference is used to construct the cyclic residual. .
[0139] The loop closure detection employs a two-layer verification mechanism. The first layer calculates the similarity between keyframes based on the Bag-of-Words (BoW) model. When the similarity exceeds a preset threshold, the second layer of verification is triggered. The second layer uses an algorithm to eliminate mismatched feature points and calculates the fundamental matrix to verify spatial geometric consistency, thereby reliably confirming loop closure relationships and effectively suppressing the cumulative error that increases with system runtime.
[0140] The steps for constructing the objective function include: integrating the above residuals to construct the objective function: ,in, This is a pose parameter vector, containing rotation and translation vectors, representing the pose state of the UAV in the world coordinate system. These are the information matrices corresponding to the visual reprojection residual, the pre-integration residual, and the loop closure detection constraint residual, respectively. The element values are determined based on factors such as the sensor noise model and the observation accuracy of feature points and map points. They are used to measure the weight of different residual terms and highlight the constraint effect of high-precision observation. These are IMU regularization parameters used to balance data items (residual sums) with regularization items. This contributes to preventing overfitting during the optimization process; The regularization matrix is designed based on the prior information of the pose parameters to further constrain the rationality of the pose solution.
[0141] The steps involved in the iterative optimization using the LM algorithm include: initializing the damping factor. =0.1, calculate the Jacobian matrix in each iteration. and Hessian matrix Incremental solution is obtained by Cholesky decomposition. When the convergence condition is met, optimization stops, and the optimized pose sequence is output. This pose sequence serves as the precise positioning input for UAV hovering control, providing crucial support for ensuring stable hovering of the UAV in satellite-denied environments.
[0142] The keyframe capacity limitation steps include: setting the maximum keyframe capacity to 500 frames, and including keyframe poses within the window. and related map points When the number of keyframes reaches a threshold, to avoid excessive consumption of computing resources, the keyframes within the window need to be streamlined. To this end, a multi-dimensional keyframe scoring system was constructed, encompassing time decay scoring. Isolation score And position difference score The time decay score measures the contribution of keyframes to the current state based on their relative freshness; the calculation formula is as follows. ,in This represents the ID difference between the newly added keyframe and the candidate deleted keyframe. The isolation score focuses on the spatial correlation between the newly added keyframe and other keyframes, measured by the number of shared map points. Quantification is performed, and the calculation method is as follows: The pose difference score is based on the pose difference between newly added keyframes and candidate deleted keyframes. The calculation method is as follows: Ultimately, the overall score... The keyframes with the lowest scores are deleted first, thereby achieving the goal of controlling the keyframe capacity and minimizing the impact on the system's positioning accuracy.
[0143] In an alternative embodiment, please refer to Figure 2 , Figure 2 This is a flowchart illustrating another satellite rejection high-altitude hovering positioning method based on visual perception and inertial measurement, provided as an embodiment of the present invention. Figure 2 As shown in the embodiments of this application, the satellite rejection high-altitude hovering positioning method based on visual perception and inertial measurement includes:
[0144] After the system starts up, step S201 is executed to initialize binocular vision.
[0145] Next, step S202 is executed to send the initialization data to the tracking module.
[0146] Next, step S203 is executed to determine whether the current altitude of the drone is greater than 10m.
[0147] If the current altitude of the drone is greater than 10m, proceed to step S204 to switch to monocular + IMU mode.
[0148] If the current altitude of the drone is no more than 10m, then proceed to step S210 to maintain binocular + IMU mode.
[0149] After executing step S204 or S210, step S205 is executed to perform pose estimation.
[0150] Next, step S206 is executed to add keyframe judgment.
[0151] Next, step S207 is executed: local mapping and sliding window optimization.
[0152] Next, step S208 is executed to determine whether the number of keyframes exceeds the limit.
[0153] If the number of keyframes exceeds the limit, proceed to step S209 to delete the lowest stable keyframe.
[0154] If the keyframe does not exceed the limit or after S209 is completed, proceed to step S211 to perform loop closure detection.
[0155] Then, S212 is executed to perform graph optimization and global optimization, and finally the pose is output.
[0156] Thus, the satellite-denied high-altitude hovering positioning method proposed in this application, by fusing camera vision with the IMU inertial measurement unit, effectively solves the problems of low positioning accuracy and easy positioning failure in degraded environments of single-sensor positioning systems. Utilizing the characteristics of monocular and binocular vision algorithms, a visual mode switching strategy is introduced to effectively balance the positioning performance of high- and low-altitude systems. Furthermore, the IMU can sense its own motion information; after visual fusion with IMU data, the impact of dynamic objects on visual positioning is mitigated to some extent. When visual tracking fails, the inertial measurement unit can operate independently for a period of time to wait for visual tracking to recover, improving the system's robustness in complex environments. The keyframe capacity control strategy effectively limits the total number of keyframes, greatly improving the system's positioning pose update frequency. Therefore, this invention overcomes the problems of low positioning accuracy, easy tracking loss in degraded environments, and low pose update frequency during long-term operation in existing UAV high-altitude hovering technologies under denial conditions, realizing a UAV high-altitude hovering method that does not rely on GNSS signals and possesses high frequency, high accuracy, and high robustness.
[0157] This application discloses a satellite-denied high-altitude hovering positioning method based on visual perception and inertial measurement. By fusing camera vision with an IMU (Inertial Measurement Unit), it effectively solves the problems of low positioning accuracy and easy positioning failure in degraded environments inherent in single-sensor positioning systems. Utilizing the characteristics of monocular and binocular vision algorithms, a visual mode switching strategy is introduced to effectively balance the positioning performance of high- and low-altitude systems. Furthermore, the IMU can sense its own motion information; after fusing visual data with IMU data, the impact of dynamic objects on visual positioning is mitigated to some extent. When visual tracking fails, the inertial measurement unit can operate independently for a period of time while visual tracking recovers, improving the system's robustness in complex environments. A keyframe capacity control strategy effectively limits the total number of keyframes, significantly increasing the system's positioning pose update frequency. Therefore, this invention overcomes the problems of low positioning accuracy, easy tracking loss in degraded environments, and low pose update frequency during long-term operation in existing UAV high-altitude hovering technologies under denial conditions. It achieves a UAV high-altitude hovering method that does not rely on GNSS signals and possesses high frequency, high accuracy, and high robustness.
[0158] Compared with existing UAV high-altitude positioning technologies, the advantages of this invention are as follows: It employs a monocular and binocular vision switching strategy, tightly coupling visual perception and IMU (Inertial Measurement Unit) data. Initialization is performed using a binocular vision-inertial scheme, switching to a monocular vision-inertial scheme at high altitudes, effectively balancing the UAV's positioning performance at both high and low altitudes. Furthermore, due to the complementarity between visual and IMU data, the tight coupling method effectively solves the motion blur problem of the camera when the UAV flies at high speeds, the long-term data drift problem of IMU inertial navigation, and the problem of visual feature matching errors in complex dynamic scenes. In the backend optimization process, three types of observation residuals are integrated: visual reprojection residual, IMU pre-integration residual, and loop closure constraint residual. This effectively combines the rich visual texture and good matching effect of camera SLAM with the ability of IMU inertial navigation to estimate its own motion and be unaffected by dynamic objects, improving the system's positioning performance in scenarios where a single sensor is not suitable. Furthermore, even if one of the single sensors, namely vision or IMU, fails, this system can still complete the localization process through the other sensor until the system recovers, greatly improving the robustness of the system in complex scenarios. A practical and effective keyframe capacity control strategy has been introduced, and a set of keyframe scoring strategies applicable to all scenarios has been designed. This strategy can efficiently control the keyframe capacity while ensuring the system's localization accuracy and output frequency. The system's output pose frequency is no longer affected by the algorithm, and it can still output high-frequency and high-precision poses even during ultra-long-term operation.
[0159] In summary, this application overcomes the problems of low positioning accuracy, easy tracking loss in degraded environments, and low pose update frequency during long-term operation of existing UAV high-altitude hovering technology under denial conditions. It realizes a UAV high-altitude hovering method that does not rely on GNSS signals and has high frequency, high precision, and high robustness.
[0160] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile memory and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM). The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, artificial intelligence (AI) processors, etc., and are not limited to these.
[0161] It should be understood that although the steps in the flowcharts of the embodiments described above are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some steps in the flowcharts of the embodiments described above may include multiple steps or multiple stages. These steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these steps or stages is not necessarily sequential, but can be performed alternately or in turn with other steps or at least some of the steps or stages of other steps.
[0162] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this application.
[0163] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of this patent application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this application should be determined by the appended claims.
Claims
1. A satellite rejection high-altitude hovering positioning method based on visual perception and inertial measurement, characterized in that, This is applied to an unmanned aerial vehicle (UAV) control system, which includes a front-end processing module and a back-end processing module. The front-end processing module includes a pre-built pose constraint model, and the back-end processing module is used for precise positioning. The front-end processing module performs the following steps: ORB feature detection and extraction on the ground environment image captured by the downward-looking binocular camera to obtain the spatial coordinate information of the feature points, and scale-invariant feature matching is achieved by combining the ground environment image pyramid; a keyframe dictionary is constructed based on the bag-of-words model, and loop closure detection is completed by calculating the similarity of the feature points; initial pose estimation is achieved based on the PNP algorithm and IMU pre-integration to generate an initial estimated pose sequence; and the binocular camera visual data and IMU inertial measurement data are fused using a tight coupling method to construct motion equations and observation equations. The backend processing module performs the following steps: it uses a sliding window optimization algorithm to optimize the initial estimated pose sequence in real time to eliminate accumulated errors; and it uses the LM optimization algorithm to solve for the maximum a posteriori probability estimate to determine the target's high-altitude hovering positioning pose. The backend processing module specifically performs the following steps: Calculate the visual reprojection residual, IMU pre-integration residual, and loop closure residual, and calculate the total residual value based on the pre-established objective function; The pose parameters are iteratively optimized using the LM algorithm, and the solution is accelerated by decomposing the sparse matrix using the Schur complement, until the total residual converges to a preset second threshold. The visual reprojection residual is calculated using the following formula: ; The IMU pre-integration residual is calculated using the following formula. ; Calculate the closure residual using the following formula. ; in, Let be the pixel coordinates of the i-th 2D image observation point. Let be the camera pose of the j-th frame, represented by the homogeneous transformation matrix. Let represent the camera pose in the k-th frame, expressed as a homogeneous transformation matrix. The 3D spatial coordinates of the i-th 2D point The estimated relative pose of the IMU from frame j to frame k. For camera projection function, The pose of the j-th frame obtained by loop closure matching; The objective function is: ; in, For pose parameter vectors, The damping factor, For regularization matrix, , , This is the information matrix corresponding to the residuals; The sliding window optimization algorithm sets the maximum keyframe capacity of the window to 500 frames. When the number of keyframes reaches the maximum value, the algorithm filters keyframes by summing the time decay score, isolation score, and pose difference score of the candidate keyframes to be deleted, and deletes the keyframe with the lowest score. The time decay score is calculated using the following formula: ; in, The difference between the IDs of the newly added keyframe and the candidate deleted keyframe; The isolation score is calculated using the following formula: ; in, The number of shared map points for newly added keyframes and candidate deleted keyframes; The pose difference score is calculated using the following formula: ； in, The pose difference between the newly added keyframe and the candidate deleted keyframe.
2. The method according to claim 1, characterized in that, The method further includes: performing binocular vision initialization in a static state; The steps for initializing binocular vision in a static state include: An initial ground feature map is constructed using binocular parallax triangulation. The gravity vector is estimated using acceleration data from the IMU when it is stationary, and the rotational component of the camera attitude is constrained to eliminate ambiguity of pure rotation. Cross-correlation analysis was used to calibrate the time offset between the image and the IMU data.
3. The method according to claim 1, characterized in that, After feature matching, the front-end processing module performs the following steps: The PNP algorithm is used to estimate the initial pose of the camera, and the Random Sample Consensus (RANSAC) algorithm is used to remove mismatched feature point pairs. Construct pose optimization equations using IMU pre-integration data; Iterative optimization of poses in adjacent frames is performed to output inter-frame pose transformation matrix and generate initial estimated pose sequence; The pose optimization equation is as follows: ； in, Let be the pose matrix to be determined. For image feature points, For 3D map points, For projection function, For IMU pre-integration pose, For weight fusion.
4. The method according to claim 1, characterized in that, The orientation of the feature point is calculated using the following formula: Define the image block moments as: ; in, , For pixel coordinates, Grayscale value; The orientation angle of the feature point is calculated using the following formula: ； in, and These represent the gray-level weighted sums of the image patch in the image height and width directions, respectively, to achieve rotation-invariant matching of feature points.
5. The method according to claim 1, characterized in that, The equation of motion is: ； in, Let k be the state vector at time k. For IMU measurements, This is process noise; The observation equation is as follows: ； in, These are visual feature observations. To observe noise; The method further includes: using extended Kalman filtering to update the state vector estimation, wherein the state vector includes position, velocity, attitude, sensor bias, and gravity vector.
6. The method according to claim 1, characterized in that, The loop closure detection employs a two-layer verification mechanism, specifically including: Keyframe similarity is calculated using BoW vectors, and it is determined whether the keyframe similarity is greater than a first preset threshold. If the keyframe similarity is greater than the first preset threshold, then the RANSAC algorithm is used to remove mismatched feature points, the fundamental matrix is calculated to verify spatial geometric consistency, and the loop relationship is confirmed.
7. The method according to claim 1, characterized in that, The method further includes: executing a positioning mode adaptive switching mechanism; The adaptive switching mechanism for positioning modes performs the following steps: When the drone flies below the altitude threshold, the binocular camera and IMU fusion positioning mode is activated. When the drone's flight altitude is higher than or equal to the altitude threshold, it switches to a monocular camera and IMU fusion positioning mode. During the switching process from the binocular camera and IMU fusion positioning mode to the monocular camera and IMU fusion positioning mode, a smooth transition algorithm is used to maintain positioning continuity.
8. The method according to claim 1, characterized in that, The method further includes: When the number of feature points detected is lower than the third threshold or the number of point pairs matched with the map tracking in the current frame is lower than the preset threshold, the pure IMU positioning mode is activated, and the IMU drift is suppressed by the zero-speed correction ZUPT algorithm until the visual features are restored and then switched back to the fusion mode.