Satellite denial high-altitude hovering positioning method based on laser and visual inertia

Abstract

The invention provides a satellite denial high-altitude hovering positioning method based on laser and visual inertia, which comprises the following steps: fusing data of a laser radar, a visible light camera and an IMU (Inertial Measurement Unit) by adopting an error state iterative Kalman filtering method to realize high-precision pose estimation of an unmanned aerial vehicle under a satellite denial condition; iMU state updating is utilized to obtain predicted pose estimation, through matching of laser radar point cloud and a voxel map, rough pose estimation is obtained, and according to luminosity matching of visible light image blocks, accurate pose estimation is determined; a least recently used mechanism is introduced into a laser radar voxel map updating module and a visual feature map, and the map scale is limited by dynamically eliminating map data which are not used for a long time, so that after a single sensor fails, other sensors continue to complete the system pose estimation capability, and the system pose estimation accuracy is improved. And robust positioning data is provided for the unmanned aerial vehicle in a single sensor degradation environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle positioning and navigation, and particularly to a satellite denial high-altitude hovering positioning method based on laser and visual inertia. BACKGROUND

[0002] In the field of unmanned aerial vehicle positioning and navigation, especially in satellite denial environments such as urban canyons, electromagnetic interference areas and other scenarios, traditional positioning schemes relying on global navigation satellite system (GNSS) signals face severe challenges. In these scenarios, GNSS signals are easily blocked by buildings, trees, electromagnetic interference or human deception, and the accuracy of GNSS positioning schemes is poor or even completely invalid, and in extreme cases, it can even cause a crash.

[0003] Simultaneous localization and mapping (SLAM) is the main way to solve the problem of unmanned aerial vehicle positioning in satellite denial environments. However, existing SLAM schemes have many limitations: pure inertial navigation (INS) can work independently, but it has the problem of rapid divergence of cumulative error over time. Long-term operation of a consumer-grade inertial measurement unit (IMU) will produce a large drift, and in high-altitude flight, the sensitivity of the IMU to air flow disturbance and body vibration is significantly increased, which will further exacerbate error accumulation; monocular visual positioning lacks scale information and relies on structure from motion (SfM) algorithms to estimate scale, but in high-altitude scenarios, the distance between ground feature points is large and the texture similarity is high, which can easily lead to scale estimation bias; binocular vision can directly calculate depth, but its accuracy is limited by the baseline length, and is not suitable for high-altitude unmanned aerial vehicle positioning scenarios; laser radar (LiDAR) can obtain high-precision three-dimensional point clouds, but its detection resolution is limited, and it is difficult to capture ground detail features in high-altitude flight, and in bad weather such as rain, fog and dust, the laser beam is easily scattered. Existing multi-sensor fusion SLAM schemes attempt to combine visual and inertial or laser and inertial data, but do not consider the dynamic changes in the observation accuracy of both in high-altitude scenarios, such as sparse laser point clouds and blurred visual features, and traditional schemes do not dynamically manage map data, so the size of the map will increase linearly with running time, and the positioning system is very likely to crash due to memory overflow, which severely limits the long-term operation capability of unmanned aerial vehicles. SUMMARY

[0004] Therefore, the purpose of the present application is to provide a satellite denial high-altitude hovering positioning method based on laser and visual inertia, which can at least solve the problems of unlimited growth of map size, large memory occupation and insufficient positioning accuracy and robustness of traditional multi-sensor fusion SLAM positioning systems, and realize a high-altitude positioning technology that is independent of GNSS signals, has high precision, high robustness and low memory occupation, and provides reliable protection for the stable operation of unmanned aerial vehicles in high-altitude complex environments.

[0005] The embodiment of the application provides a satellite denial high-altitude hovering positioning method based on laser and visual inertia, comprising:

[0006] An error state iterative Kalman filtering method is used to fuse lidar, visible light camera and IMU inertial measurement unit data, so that high-precision pose estimation of the unmanned aerial vehicle under the satellite denial condition is realized.

[0007] The predicted pose estimation is obtained by IMU state updating, the rough pose estimation is obtained by matching the lidar point cloud and the voxel map, and the accurate pose estimation is determined according to the photometric matching of the visible light image block.

[0008] The least recently used mechanism is introduced into the lidar voxel map updating module and the visual feature map, the map size is limited by dynamically eliminating long-term unused map data, so that the ability of other sensors to continue to complete the system pose estimation after the failure of a single sensor is realized, and robust positioning data of the unmanned aerial vehicle in the single sensor degradation environment is provided.

[0009] Optionally, the step of IMU state updating comprises:

[0010] The predicted pose estimation is determined by processing and state updating of the IMU inertial measurement unit data.

[0011] The processing and state updating of the IMU inertial measurement unit data comprises attitude updating, error compensation and position or speed updating.

[0012] The attitude updating is performed by the following formula:

[0013] ;

[0014] The attitude updating formula is that the angular velocity measurement value of the IMU at time is , the attitude quaternion is updated after deducting the zero offset , represents quaternion multiplication, and the attitude quaternion at time is discretized by median integration ;

[0015] The position or speed updating is performed by the following formula: is the IMU sampling interval, is the quaternion exponential mapping, is the angular velocity measurement value at time , and is the zero offset at time

[0016] Optionally, the position or speed updating is performed by the following formula:​

[0017] , ;

[0018] wherein the position or velocity update formula is the acceleration measurement value of the IMU at time , after deducting the zero offset , is rotated by the attitude, the velocity and the position update formula, is the quaternion conjugate, is the gravity acceleration vector;

[0019] wherein the discrete velocity at time , position at time , is the acceleration measurement value at time , is the acceleration zero offset at time and is the attitude rotation matrix at time and .

[0020] Optionally, error compensation is performed by the following formula:

[0021] ; ;

[0022] wherein is the zero offset random walk noise, which is subject to Gaussian distribution.

[0023] Optionally, the step of matching the lidar point cloud with the voxel map comprises:

[0024] solving the optimal pose by minimizing the residual error of the point cloud and the voxel;

[0025] wherein the step of solving the optimal pose by minimizing the residual error of the point cloud and the voxel comprises:

[0026] finding the voxel grid in which the point cloud in the current world coordinate system is located, and calculating the depth residual error, wherein the voxel map is composed of a plurality of voxel units;

[0027] solving the optimal pose by minimizing the residual error square sum, and solving the rough position and the rough attitude by using the Gauss-Newton method for iteration, and calculating the Jacobian matrix of the residual error with respect to the displacement and rotation increment in the iteration process, and constructing the increment equation.

[0028] Optionally, determining the precise pose estimate based on photometric matching of visible light image patches includes:

[0029] Image patch sampling is performed in the reference image;

[0030] The world coordinates of the pixels in the reference image are projected onto the pixel coordinates of the current image, and the grayscale of the image block is obtained by bilinear interpolation.

[0031] Based on the photometric residual of the image block grayscale and pose optimization, the photometric residual is calculated, the square of the residual is minimized and the optimal pose is solved. The Gauss-Newton method is used for iterative solution. During the iteration process, the Jacobian matrix of the residual with respect to the displacement and rotation increments is calculated, the increment equation is constructed, and the accurate pose estimate is obtained by solving.

[0032] The following formula represents the process of sampling image patches in the reference image:

[0033] ;

[0034] Among them, time The reference image is The current image is , For pixel coordinates, Grayscale value;

[0035] The world coordinate projection value corresponding to a pixel in the reference image is determined by the following formula:

[0036] ;

[0037] Wherein, the world coordinates corresponding to the pixels in the reference image are The current pose is , For the camera intrinsic parameter matrix, This is the extrinsic rotation matrix from the camera to the body coordinate system;

[0038] The photometric residual is calculated using the following formula:

[0039] ;

[0040] The optimal pose is solved by minimizing the sum of squared residuals using the following formula:

[0041] ;

[0042] Among them, precise pose estimation is Current image patch grayscale .

[0043] Optionally, the pose estimation step includes: fusing the IMU predicted pose, the laser coarse pose, and the visual precise pose to obtain a precise pose estimate;

[0044] The steps involved in fusing IMU-predicted pose, coarse laser pose, and precise visual pose to obtain accurate pose estimation include:

[0045] Define the error state;

[0046] Based on the IMU state update results, the prediction error state covariance matrix is ​​predicted.

[0047] Based on the laser observation model and the laser Kalman gain formula, the laser covariance and error state are updated. After the update, the error state is fed back to the original state to complete one laser observation fusion. Based on the visual observation model and the visual Kalman gain formula, the visual covariance and error state are updated.

[0048] Based on the updated covariance and error state, the accurate pose estimate is determined.

[0049] Fusion pose From the updated state The conversion yields high-precision pose estimation.

[0050] Optionally, the error state is defined as follows:

[0051] ;

[0052] The preset real state is ,in Indicates location, Indicates a gesture, Indicates speed, and Representing angular velocity travel and acceleration travel respectively, the predicted state is: ;

[0053] Among them, the error state covariance matrix The prediction formula is:

[0054] ;

[0055] Among them, State transition matrix The noise input matrix, The noise covariance matrix of the IMU;

[0056] The laser observation model is as follows:

[0057] ;

[0058] in, For the observation matrix, For laser observation noise, Position estimated by visual measurement, Pose estimated for visual measurements;

[0059] The laser Kalman gain is:

[0060] ;

[0061] in, Here is the laser noise covariance matrix;

[0062] The formulas for updating the laser covariance and error state are as follows:

[0063] , ;

[0064] in, For the predicted state;

[0065] The visual observation model is as follows:

[0066] ;

[0067] in, For the observation matrix, This is visual observation noise;

[0068] Wherein, the visual Kalman gain is:

[0069] ;

[0070] in, The visual noise covariance matrix;

[0071] The formulas for updating the visual covariance and error state are as follows:

[0072] , .

[0073] Optionally, the step of updating the map based on the least recently used mechanism includes:

[0074] In the LiDAR voxel map update module, if the number of voxels in the current voxel map reaches the preset maximum capacity, the usage records of each voxel in the voxel map are checked when adding newly constructed voxels to the map.

[0075] In the LiDAR voxel map update module, if the number of voxels in the current voxel map reaches the preset maximum capacity, the steps for checking the usage records of each voxel in the voxel map when adding newly constructed voxels to the map include:

[0076] Based on the least recently used mechanism, identify voxel data that have not been used for a long time;

[0077] The longest unused voxel data in the long-term unused voxel data is deleted from the voxel map to free up memory space. At the same time, the newly constructed voxels are added to the existing voxel map to update the voxel map status.

[0078] In the visual feature map, according to the least recently used mechanism, when the visual feature map reaches its maximum capacity, each time a new visual feature map element is added, the visual feature that has not been observed for the longest time is deleted, so as to ensure that the system can stably output high-frequency and high-precision estimated pose even in embedded devices.

[0079] Optionally, in nighttime scenarios, when camera visual features fail, high-precision positioning data is output based on the inertial fusion of LiDAR and IMU.

[0080] The satellite rejection high-altitude hovering positioning method based on laser and visual inertial provided in this application can at least solve the problems of unlimited map growth, large memory consumption, and insufficient positioning accuracy and robustness of traditional multi-sensor fusion SLAM positioning systems. It realizes a high-altitude positioning technology that does not rely on GNSS signals, has high precision, high robustness and low memory consumption, and provides a reliable guarantee for the stable operation of UAVs in complex high-altitude environments.

[0081] 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

[0082] 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.

[0083] Figure 1 A flowchart illustrating a satellite rejection high-altitude hovering positioning method based on laser and visual inertial measurements, provided for an embodiment of the present invention;

[0084] Figure 2A flowchart illustrating another satellite rejection high-altitude hovering positioning method based on laser and visual inertial measurement provided in an embodiment of the present invention;

[0085] Figure 3 This is a schematic diagram of an unmanned aerial vehicle (UAV) control system provided in an embodiment of the present invention. Detailed Implementation

[0086] 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.

[0087] 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.

[0088] 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.

[0089] In the field of UAV positioning and navigation, especially in satellite-denied environments such as urban canyons and areas with electromagnetic interference, traditional positioning solutions relying on Global Navigation Satellite System (GNSS) signals face severe challenges. In these scenarios, GNSS signals are easily blocked by buildings, trees, electromagnetic interference, or human deception, resulting in poor accuracy or even complete failure of GNSS positioning solutions. In extreme cases, this can even lead to UAV crashes.

[0090] Simultaneous Localization and Mapping (SLAM) is a primary approach to solving the UAV positioning problem in satellite-denied environments. However, existing SLAM solutions have several limitations: Pure Inertial Navigation (INS), while capable of independent operation, suffers from the problem of accumulated errors rapidly diverging over time. Consumer-grade Inertial Measurement Units (IMUs) experience significant drift over extended periods, and their sensitivity to airflow disturbances and fuselage vibrations increases dramatically at high altitudes, further exacerbating error accumulation. Monocular vision positioning, lacking scale information, relies on Structure of Motion (SfM) algorithms to estimate scale; in high-altitude scenarios, the large spacing between ground feature points and high texture similarity easily lead to scale estimation errors. While binocular vision can directly calculate depth, its accuracy is limited by baseline length, making it unsuitable for high-altitude UAV positioning scenarios. LiDAR can acquire high-precision 3D point clouds, but its detection resolution is limited, making it difficult to capture detailed ground features at high altitudes, and the laser beam is easily scattered in adverse weather conditions such as rain, fog, and sandstorms. While existing multi-sensor fusion SLAM solutions attempt to combine visual and inertial or laser and inertial data, they do not consider the dynamic changes in the observation accuracy of both in high-altitude scenarios, such as sparse high-altitude laser point clouds and blurred visual features. Furthermore, traditional solutions do not dynamically manage map data, and the map size grows linearly with the running time. The positioning system is very prone to crashing due to memory overflow, which severely limits the long-term operation capability of UAVs.

[0091] The satellite denial high-altitude hovering positioning method based on laser and visual-inertial proposed in this application can at least solve the problems of unlimited map growth, large memory consumption, and insufficient positioning accuracy and robustness of traditional multi-sensor fusion SLAM positioning systems. It realizes a high-altitude positioning technology that does not rely on GNSS signals, has high accuracy, high robustness and low memory consumption, and provides a reliable guarantee for the stable operation of UAVs in complex high-altitude environments.

[0092] Please see Figure 1 , Figure 1 This is a flowchart illustrating a satellite rejection high-altitude hovering positioning method based on laser and visual-inertial methods, provided as an embodiment of this application. Figure 1 As shown in the embodiments of this application, the satellite rejection high-altitude hovering positioning method based on laser and visual inertial navigation includes:

[0093] S101. The error state iterative Kalman filter method is used to fuse data from lidar, visible light camera and IMU inertial measurement unit to achieve high-precision pose estimation of UAV under satellite denial conditions.

[0094] S102. The predicted pose estimate is obtained by updating the IMU state. A rough pose estimate is obtained by matching the lidar point cloud with the voxel map. The accurate pose estimate is determined by matching the photometric values ​​of visible light image blocks.

[0095] S103. Introduce a Least Recently Used (LRU) mechanism in the LiDAR voxel map update module and visual feature map. By dynamically eliminating long-term unused map data, the map size is limited. This enables other sensors to continue to complete the system pose estimation after a single sensor fails, providing robust positioning data for UAVs in environments where a single sensor is degraded.

[0096] Specifically, the IMU state update step includes: determining the predicted pose estimate by processing and updating the IMU inertial measurement unit data.

[0097] The processing and status update of the IMU inertial measurement unit data includes: attitude update, error compensation, and position or velocity update.

[0098] The attitude is updated using the following formula:

[0099] ;

[0100] The attitude update formula is the IMU at time [time]. The measured angular velocity value is , deducting zero bias After that, attitude quaternions The update formula, To represent quaternion multiplication, discretization using mean value integral is performed. The attitude quaternion of time: ;

[0101] in, The IMU sampling interval For quaternion exponent mapping, for The angular velocity measurement at time t. for Zero bias at any given moment.

[0102] In some embodiments, position or velocity can be updated using the following formula:

[0103] , ;

[0104] The position or velocity update formula is given by the IMU at time [time]. The acceleration measurement value , deducting zero bias Afterwards, through attitude rotation, speed and location The update formula, For quaternion conjugate, This is the vector of gravitational acceleration.

[0105] Among them, after discretization Speed ​​of time , Location at any moment , for The acceleration measurement at time [time]. for The acceleration at any given moment is zero bias. and For a moment and The attitude rotation matrix.

[0106] Specifically, error compensation can be performed using the following formula:

[0107] ; ;

[0108] in, It is zero-biased random walk noise that follows a Gaussian distribution.

[0109] In some embodiments, the step of matching LiDAR point clouds with voxel maps includes: solving for the optimal pose by minimizing the residuals between the point clouds and voxels.

[0110] The steps for finding the optimal pose by minimizing the residual between the point cloud and the voxel include:

[0111] Find the voxel in the current world coordinate system and calculate the depth residual. The voxel map is composed of several voxel units.

[0112] The optimal pose is solved by minimizing the sum of squared residuals. The Gauss-Newton method is used for iterative solution. During the iteration process, the Jacobian matrix of the residuals with respect to the displacement and rotation increments is calculated, and the incremental equation is constructed to obtain the coarse position and coarse pose.

[0113] Specifically, based on photometric matching of visible light image patches, precise pose estimation is determined, including:

[0114] Image patch sampling is performed in the reference image;

[0115] The world coordinates of the pixels in the reference image are projected onto the pixel coordinates of the current image, and the grayscale of the image block is obtained by bilinear interpolation.

[0116] Based on the photometric residuals of the image patch grayscale and pose optimization, the photometric residuals are calculated, the squared residuals are minimized, and the optimal pose is solved. The Gauss-Newton method is used for iterative solution. During the iteration process, the Jacobian matrix of the residuals with respect to displacement and rotation increments is calculated, the increment equation is constructed, and the accurate pose estimate is obtained by solving.

[0117] The following formula represents the process of sampling image patches in the reference image:

[0118] ;

[0119] Among them, time The reference image is The current image is , For pixel coordinates, This is the grayscale value.

[0120] The world coordinate projection value corresponding to a pixel in the reference image is determined by the following formula:

[0121] ;

[0122] Wherein, the world coordinates corresponding to the pixels in the reference image are The current pose is , For the camera intrinsic parameter matrix, This is the extrinsic rotation matrix from the camera to the body coordinate system.

[0123] The photometric residual is calculated using the following formula:

[0124] ;

[0125] The optimal pose is solved by minimizing the sum of squared residuals using the following formula:

[0126] ;

[0127] Among them, precise pose estimation is Current image patch grayscale .

[0128] Specifically, the pose estimation step includes: fusing the IMU predicted pose, the laser coarse pose, and the visual precise pose to obtain a precise pose estimate.

[0129] The steps involved in fusing IMU-predicted pose, coarse laser pose, and precise visual pose to obtain accurate pose estimation include:

[0130] Define the error state;

[0131] Based on the IMU state update results, the prediction error state covariance matrix is ​​predicted.

[0132] Based on the laser observation model and the laser Kalman gain formula, the laser covariance and error state are updated. After the update, the error state is fed back to the original state to complete one laser observation fusion. Based on the visual observation model and the visual Kalman gain formula, the visual covariance and error state are updated.

[0133] Based on the updated covariance and error state, the accurate pose estimate is determined.

[0134] Among them, the fusion pose From the updated state The conversion yields high-precision pose estimation.

[0135] As an example, the error state is defined as follows:

[0136] ;

[0137] The preset real state is ,in Indicates location, Indicates a gesture, Indicates speed, and Representing angular velocity travel and acceleration travel respectively, the predicted state is: .

[0138] Among them, the error state covariance matrix The prediction formula is:

[0139] ;

[0140] in, State transition matrix The noise input matrix, This is the IMU noise covariance matrix.

[0141] The laser observation model is as follows:

[0142] ;

[0143] in, For the observation matrix, This is for laser observation noise;

[0144] The laser Kalman gain is:

[0145] ;

[0146] in, Here is the laser noise covariance matrix;

[0147] The formulas for updating the laser covariance and error state are as follows:

[0148] , ;

[0149] in, For the predicted state;

[0150] The visual observation model is as follows:

[0151] ;

[0152] in, For the observation matrix, This is visual observation noise;

[0153] Wherein, the visual Kalman gain is:

[0154] ;

[0155] in, The visual noise covariance matrix;

[0156] The formulas for updating the visual covariance and error state are as follows:

[0157] , .

[0158] Specifically, the steps for updating the map based on the least recently used mechanism include:

[0159] In the LiDAR voxel map update module, if the number of voxels in the current voxel map reaches the preset maximum capacity, the usage records of each voxel in the voxel map are checked when adding newly constructed voxels to the map.

[0160] In the LiDAR voxel map update module, if the number of voxels in the current voxel map reaches the preset maximum capacity, the steps for checking the usage records of each voxel in the voxel map when adding newly constructed voxels to the map include:

[0161] Based on the least recently used mechanism, identify voxel data that have not been used for a long time;

[0162] The longest unused voxel data in the long-term unused voxel data is deleted from the voxel map to free up memory space. At the same time, the newly constructed voxels are added to the existing voxel map to update the voxel map status.

[0163] In the visual feature map, according to the least recently used mechanism, when the visual feature map reaches its maximum capacity, each time a new visual feature map element is added, the visual feature that has not been observed for the longest time is deleted, so as to ensure that the system can stably output high-frequency and high-precision estimated pose even in embedded devices.

[0164] In particular, in nighttime scenarios, when camera visual features fail, high-precision positioning data is output based on the fusion of LiDAR and IMU inertial data.

[0165] 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 laser and visual inertial methods provided in an embodiment of this application.

[0166] like Figure 2 As shown, after the system starts, step S201 is executed to read sensor data.

[0167] Next, step S202 is performed to convert the point cloud into voxels; step S208 is performed to preprocess the IMU; and step S210 is performed to construct visual image blocks.

[0168] After step S208, step S209 is executed, IMU prediction.

[0169] After step S202, step S203 is executed to perform point cloud voxel map matching.

[0170] After step S210, step S211 is executed to perform visual feature matching.

[0171] After steps S203, S209 or S211, step S204 is executed to perform ESIKF fusion optimization.

[0172] Next, step S205 is executed to check the map capacity to determine whether the map capacity is greater than the preset maximum capacity.

[0173] If the map capacity is greater than the preset maximum capacity, then execute step S212, LRU mechanism: delete the least used voxel and step S213, LRU mechanism: delete the least used feature.

[0174] If the map capacity is less than or equal to the preset maximum capacity, then proceed to step S206 to update the map.

[0175] After step S206, pose output is performed, and step S201 is executed again to process the next frame.

[0176] As an example, the satellite-denied high-altitude hovering positioning method based on laser and visual-inertial technology proposed in this application is applied to an unmanned aerial vehicle (UAV) control system. Please refer to [link to relevant documentation]. Figure 3The UAV control system includes: an IMU state update module, a lidar point cloud and voxel map matching module, a visible light image patch photometric matching module, an error state iterative Kalman filter (ESIKF) fusion module, and a least recently used (LRU) mechanism map update module.

[0177] The IMU state update module processes and updates the state of the IMU measurement data to obtain the predicted pose estimate.

[0178] The IMU state update module performs the following steps: Calculate the angular velocity integral (attitude update): Assume the IMU is at time t. The measured angular velocity value is , deducting zero bias After that, attitude quaternions The update formula is: ,in Quaternion multiplication, after discretization using mean value integral: In the formula The IMU sampling interval Quaternion exponential mapping; Calculate acceleration integral (position / velocity update): Assume the IMU at time... The acceleration measurement value , deducting zero bias After attitude rotation (converting the acceleration in the body coordinate system to the world coordinate system), the velocity... and location The update formula is: , ,in For quaternion conjugate, This is the vector of gravitational acceleration. After discretization: , Error compensation is performed by introducing an IMU zero-bias drift model, with the zero-bias update formula as follows: , ,in The noise is zero-biased random walk and follows a Gaussian distribution. Error correction can be achieved using the above formula, resulting in an accurate predicted pose.

[0179] The lidar point cloud and voxel map matching module matches the point cloud data collected by the lidar with the voxel map to obtain a rough pose estimate.

[0180] The core of the lidar point cloud and voxel map matching module is to solve for the optimal pose by minimizing the residual between the point cloud and the voxel. Specific steps include: Voxel map representation: The voxel map consists of several voxel units. Composition, in which Let the coordinates be the center coordinates of the voxel. The average depth of the point cloud within a voxel; point cloud transformation and residual calculation: finding the point cloud in the current world coordinate system. The voxel Calculate depth residual Pose optimization: Solving for the optimal pose by minimizing the sum of squared residuals. ,in The confidence weights for the point cloud are obtained using the Gauss-Newton method iteratively. During the iteration process, the residuals are calculated with respect to the displacement. and rotation increment Jacobian matrix Construct incremental equations The rough pose estimate is obtained by solving the problem. .

[0181] The visible light image block photometric matching module is used to perform photometric matching on image blocks captured by a visible light camera to obtain accurate pose estimation.

[0182] Specifically, the core of the visible light image patch photometric matching module is to solve the pose by constraining the grayscale consistency of the image patch. The specific steps include: image patch sampling: setting the time... The reference image is The current image is ,exist Selecting image patches ,in For pixel coordinates, The value is grayscale; pixel reprojection: the world coordinates of the pixels in the reference image patch are... Based on the current pose The pixel coordinates projected onto the current image are In the formula For the camera intrinsic parameter matrix, The external parameter rotation matrix from the camera to the body coordinate system is used to obtain the grayscale of the current image patch through bilinear interpolation. Photometric residuals and pose optimization calculations for photometric residuals Solve for the optimal pose by minimizing the sum of squared residuals. The Gauss-Newton method is used for iterative solution. During the iteration process, the residuals are calculated with respect to displacement. and rotation increment Jacobian matrix Construct incremental equations The accurate pose estimate is obtained by solving the problem. .

[0183] Among them, the Error State Iterative Kalman Filter (ESIKF) fusion module is used to obtain the final pose estimate based on the fused IMU predicted pose, the laser coarse pose, and the visual precise pose.

[0184] Specifically, the Error State Iterative Kalman Filter (ESIKF) fusion module performs the following steps: Error State Definition: Let the true state be... The predicted state is Define error state Prediction step: Based on the IMU state update results, the error state covariance matrix... The prediction formula is: ,in State transition matrix The noise input matrix, For IMU noise covariance matrix; Update step: Laser observation update: Laser observation model ,in For the observation matrix, This refers to noise from laser observation. The laser noise covariance matrix and the Kalman gain are given. Update the covariance and error states: , In the formula To predict the state, the updated state is fed back to the original state, completing one laser observation fusion.

[0185] Optionally, visual observation updates can also be performed, with specific steps including: visual observation model ,in For the observation matrix, This is visual observation noise; The visual noise covariance matrix and Kalman gain are given. Update the covariance and error states: , Final fusion pose From the updated state The conversion yields high-precision pose estimation.

[0186] In the LRU (Least Recently Used) mechanism map update module, to effectively control map size and reduce system memory usage, this application introduces an LRU mechanism in both the LiDAR voxel map update module and the visual feature map. By dynamically eliminating long-term unused map data, it ensures that the map remains within a reasonable size range, avoiding excessive memory consumption caused by too much map data. In the LiDAR voxel map update module, when the voxel map capacity reaches its maximum and new voxel elements need to be added to the map, the system uses the LRU mechanism to determine which voxel data has been unused for a long time and eliminates them. Simultaneously, the existing voxel map is updated, incorporating the new point cloud data. A similar mechanism is used in the visual feature map, deleting long-unobserved visual features to maintain the map's simplicity and effectiveness.

[0187] Furthermore, this application exhibits strong robustness. Even after a single sensor fails, other sensors can still continue to perform pose estimation, providing reliable positioning data for the UAV in environments where a single sensor is degraded. When the LiDAR fails, the system can continue pose estimation using data from the IMU and visible light camera. By appropriately fusing the predicted pose from the IMU with the accurate pose obtained through photometric matching of visible light image patches, a certain level of positioning accuracy can still be guaranteed, enabling the UAV to fly safely in complex environments. When the visual sensor fails, such as in nighttime scenes, the IMU predicted pose is similarly fused with the LiDAR voxel map matching pose estimation to provide robust and reliable estimated pose.

[0188] This application discloses a satellite-denied high-altitude hovering positioning method based on laser and visual-inertial systems. By fusing lidar, camera vision, and IMU (Inertial Measurement Unit), it effectively solves the problems of low positioning accuracy and easy positioning failure in degraded environments associated with single-sensor positioning systems. By introducing an LRU (Least Recently Used) mechanism, the system maintains a low memory footprint throughout long-term operation, significantly improving the algorithm's stability in long-term scenarios. The error state iterative Kalman filter method is used to fuse data from multiple sensors, fully leveraging the advantages of lidar, visible light camera, and IMU to achieve high-precision pose estimation. In satellite-denied environments, it can provide accurate positioning information for UAVs, ensuring stable flight and mission execution. It possesses fault tolerance capability after a single sensor failure, allowing other sensors to continue the system's pose estimation. Therefore, this invention overcomes the problems of low positioning accuracy and program crashes due to infinitely growing maps during long-term operation in existing UAV high-altitude hovering technologies under satellite denial conditions, realizing a UAV high-altitude hovering method that does not rely on GNSS signals and features high frequency, high accuracy, and high robustness.

[0189] 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.

[0190] 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.

[0191] 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.

[0192] 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.

[0193] 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 laser and visual-inertial methods, characterized in that, include: The error state iterative Kalman filter method is used to fuse data from lidar, visible light camera and IMU inertial measurement unit to achieve high-precision pose estimation of UAV under satellite denial conditions; The predicted pose estimate is obtained by using IMU state update, a coarse pose estimate is obtained by matching LiDAR point cloud with voxel map, and the precise pose estimate is determined by matching photometric values ​​of visible light image patches. A least recently used mechanism is introduced into the lidar voxel map update module and the visual feature map. By dynamically eliminating long-term unused map data to limit the map size, the system can continue to complete the system pose estimation by other sensors after a single sensor fails, providing robust positioning data for UAVs in environments where a single sensor is degraded.

2. The method according to claim 1, characterized in that, The steps for updating the IMU state include: The predicted pose estimate is determined by processing and updating the data of the IMU (Inertial Measurement Unit). The processing and state update of the IMU inertial measurement unit data includes: attitude update, error compensation, and position or velocity update; The attitude is updated using the following formula: ； The attitude update formula is the IMU at time [time]. The measured angular velocity value is , deducting zero bias After that, attitude quaternions The update formula, To represent quaternion multiplication, discretization using mean value integral is performed. The attitude quaternion of time: ; in, The IMU sampling interval For quaternion exponent mapping, for The angular velocity measurement at time t. for Zero bias at any given moment.

3. The method according to claim 2, characterized in that, Position or velocity is updated using the following formula: ， ； The position or velocity update formula is given by the IMU at time [time]. The acceleration measurement value , deducting zero bias Afterwards, through attitude rotation, speed and location The update formula, For quaternion conjugate, This is the vector of gravitational acceleration; Among them, after discretization Speed ​​of time , Location at any moment , for The acceleration measurement at time [time]. for The acceleration at any given moment is zero bias. and For a moment and The attitude rotation matrix.

4. The method according to claim 2, characterized in that, Error compensation is performed using the following formula: ； ； in, It is zero-biased random walk noise that follows a Gaussian distribution.

5. The method according to claim 1, characterized in that, The steps for matching LiDAR point clouds with voxel maps include: The optimal pose is solved by minimizing the residual between the point cloud and the voxel; The steps for finding the optimal pose by minimizing the residual between the point cloud and the voxel include: Find the voxel in the current world coordinate system and calculate the depth residual. The voxel map is composed of several voxel units. The optimal pose is solved by minimizing the sum of squared residuals. The Gauss-Newton method is used for iterative solution. During the iteration process, the Jacobian matrix of the residuals with respect to the displacement and rotation increments is calculated, and the incremental equation is constructed to obtain the coarse position and coarse pose.

6. The method according to claim 1, characterized in that, Based on photometric matching of visible light image patches, a precise pose estimate is determined, including: Image patch sampling is performed in the reference image; Project the world coordinates of the pixels in the reference image onto the pixel coordinates of the current image, and obtain the grayscale of the image block through bilinear interpolation; Based on the photometric residual of the image block grayscale and pose optimization, the photometric residual is calculated, the square of the residual is minimized and the optimal pose is solved. The Gauss-Newton method is used for iterative solution. During the iteration process, the Jacobian matrix of the residual with respect to the displacement and rotation increments is calculated, the increment equation is constructed, and the accurate pose estimate is obtained by solving. The following formula represents the process of sampling image patches in the reference image: ； Among them, time The reference image is The current image is , For pixel coordinates, Grayscale value; The world coordinate projection value corresponding to a pixel in the reference image is determined by the following formula: ； Wherein, the world coordinates corresponding to the pixels in the reference image are The current pose is , For the camera intrinsic parameter matrix, This is the extrinsic rotation matrix from the camera to the body coordinate system; The photometric residual is calculated using the following formula: ； The optimal pose is solved by minimizing the sum of squared residuals using the following formula: ； Among them, precise pose estimation is Current image patch grayscale .

7. The method according to claim 1, characterized in that, The pose estimation steps include: fusing IMU predicted pose, laser coarse pose, and visual precise pose to obtain a precise pose estimate; The steps involved in fusing IMU-predicted pose, coarse laser pose, and precise visual pose to obtain accurate pose estimation include: Define the error state; Based on the IMU state update results, predict the error state covariance matrix; Based on the laser observation model and the laser Kalman gain formula, the laser covariance and error state are updated. After the update, the error state is fed back to the original state to complete one laser observation fusion. Based on the visual observation model and the visual Kalman gain formula, the visual covariance and error state are updated. Based on the updated covariance and error state, determine the precise pose estimate; Fusion pose From the updated state The conversion yields high-precision pose estimation.

8. The method according to claim 7, characterized in that, The error state is defined as follows: ； The preset real state is ,in Indicates location, Indicates a gesture, Indicates speed, and Representing angular velocity travel and acceleration travel respectively, the predicted state is: ; Among them, the error state covariance matrix The prediction formula is: ； Among them, State transition matrix The noise input matrix, The noise covariance matrix of the IMU; The laser observation model is as follows: ； in, For the observation matrix, For laser observation noise, Position estimated by visual measurement, Pose estimated for visual measurements; The laser Kalman gain is: ； in, Here is the laser noise covariance matrix; The formulas for updating the laser covariance and error state are as follows: ， ； in, For the predicted state; The visual observation model is as follows: ； in, For the observation matrix, This is visual observation noise; Wherein, the visual Kalman gain is: ； in, The visual noise covariance matrix; The formulas for updating the visual covariance and error state are as follows: ， 。 9. The method according to claim 1, characterized in that, The steps for updating the map based on the Least Recently Used (LRU) mechanism include: In the LiDAR voxel map update module, if the number of voxels in the current voxel map reaches the preset maximum capacity, the usage records of each voxel in the voxel map are checked when adding newly constructed voxels to the map. In the LiDAR voxel map update module, if the number of voxels in the current voxel map reaches the preset maximum capacity, the steps for checking the usage records of each voxel in the voxel map when adding newly constructed voxels to the map include: Based on the least recently used mechanism, identify voxel data that have not been used for a long time; The longest unused voxel data in the long-term unused voxel data is deleted from the voxel map to free up memory space. At the same time, the newly constructed voxels are added to the existing voxel map to update the voxel map status. In the visual feature map, according to the least recently used mechanism, when the visual feature map reaches its maximum capacity, each time a new visual feature map element is added, the visual feature that has not been observed for the longest time is deleted, so as to ensure that the system can stably output high-frequency and high-precision estimated pose even in embedded devices.

10. The method according to claim 1, characterized in that, In nighttime scenarios, when camera visual features fail, high-precision positioning data is output based on the fusion of LiDAR and IMU inertial data.

Images