A method for estimating the pose of a UAV based on KAN adaptive observation covariance adjustment

Abstract

The present application provides a kind of unmanned aerial vehicle pose estimation method based on KAN adaptive observation covariance adjustment, belong to visual inertial navigation technical field.For unmanned aerial vehicle in feature sparse, matching degradation, visual observation unstable scene, traditional fixed observation noise parameter Kalman filtering method is difficult to dynamically balance the confidence of visual observation and inertial prediction, the present application introduces Kolmogorov-Arnold Network (KAN) into visual inertial filtering fusion framework, constructs the multi-dimensional observation feature vector consisting of basic visual quality, feature point spatial distribution and visual inertial consistency, and outputs visual translation observation covariance and visual rotation observation covariance by KAN covariance estimation model adaptively.Model training, the observation covariance output by KAN is substituted into Kalman filtering fusion process, to fuse the pose error between trajectory and reference trajectory as training constraint, the KAN covariance estimation model is optimized end to end, and the trained model can output visual observation covariance according to the multi-dimensional observation feature vector of current time, for dynamically adjusting the weight of visual observation in Kalman filtering observation update.The method can reduce the influence of abnormal visual observation on fusion result when visual degradation, and improve visual information utilization rate when visual observation is reliable, so as to improve the precision and robustness of unmanned aerial vehicle pose estimation.

Description

Technical Field

[0001] This invention belongs to the field of visual inertial navigation technology, specifically relating to a binocular visual inertial UAV pose estimation method based on Kolmogorov-Arnold Network (KAN) adaptive adjustment of visual observation covariance. Background Technology

[0002] When performing tasks such as autonomous navigation, path planning, and 3D reconstruction, unmanned aerial vehicles (UAVs) need to continuously obtain their position and attitude in space. Visual odometry and visual simultaneous localization and mapping (VSLAM) methods can estimate camera motion using camera images. Among these, binocular vision has significant application value in UAV pose estimation because it can directly recover scale information.

[0003] However, pure visual pose estimation is susceptible to interference from sudden changes in illumination, missing textures, motion blur, and uneven distribution of feature points, leading to decreased feature matching accuracy, large pose calculation errors, and even tracking failure in severe cases. Inertial measurement units (IMUs) possess the characteristics of high-frequency output and stable short-term accuracy, effectively compensating for the shortcomings of visual solutions in fast-moving and short-term occlusion scenarios. However, pure inertial integration will accumulate and drift over time. Therefore, fusing binocular vision with IMUs is a key technical approach to improve the robustness and accuracy of pose estimation.

[0004] Current mainstream visual-inertial fusion algorithms mostly integrate visual observations and inertial predictions within frameworks such as Kalman filtering, extended Kalman filtering, error-state Kalman filtering, or graph optimization. For example, visual-inertial odometry frameworks like VINS-Mono and VINS-Fusion rely on visual reprojection residuals, inertial pre-integration residuals, and robust kernel functions to achieve unified processing of constraints from multiple sensor sources. ORB-SLAM3, in its graph optimization architecture, characterizes observation uncertainties through feature residual weights, information matrices, and robust kernel functions. While these methods achieve relatively good pose estimation results, their visual observation uncertainties are mainly determined by manually set parameters, front-end geometric constraints, or internal weights of the optimizer, lacking an adaptive adjustment mechanism for observation covariance to address dynamic changes in the quality of the binocular vision front end.

[0005] In filter-based fusion architectures, the visual observation noise covariance matrix directly determines the Kalman gain, thus affecting the confidence weight of visual observations in the fusion update. Using fixed observation noise parameters is difficult to adapt to the dynamic fluctuations in visual observation reliability in real-world scenarios, easily leading to over-reliance on abnormal observations during visual degradation phases or failure to fully extract effective visual information when visual quality is excellent. Therefore, it is necessary to design a UAV pose estimation method that can adaptively adjust the observation covariance in real time based on visual observation reliability. Summary of the Invention

[0006] The purpose of this invention is to provide a UAV pose estimation method based on KAN adaptive observation covariance adjustment. This method adaptively outputs the visual translation observation covariance matrix and the visual rotation observation covariance matrix through a KAN covariance estimation model, and dynamically adjusts the visual observation weights during the Kalman filter fusion process, thereby improving the accuracy and robustness of UAV pose estimation in complex scenarios.

[0007] To achieve the above objectives, this invention provides a UAV pose estimation method based on KAN adaptive observation covariance adjustment, comprising the following steps:

[0008] Step 1. Acquire the left and right eye image sequences captured by the UAV's binocular camera, the angular velocity and linear acceleration data collected by the inertial measurement unit, as well as the binocular camera's intrinsic parameters, binocular baseline parameters, and extrinsic parameter calibration information between the camera and the inertial measurement unit;

[0009] Step 2. Perform ORB feature extraction, descriptor matching, and binocular depth calculation on the left and right eye images through a binocular vision front end. And obtain visual translation observation and visual rotation observation between adjacent frames through adjacent frame feature matching and pose optimization.

[0010] Step 3. Extract features that characterize the reliability of visual observation during the operation of the binocular vision front end, and construct a feature vector, including basic visual quality features, feature point grid spatial distribution features, and consistency features between visual observation and inertial prediction;

[0011] Step 4. Construct and train the KAN covariance estimation model. The model takes the feature vector as input and outputs the visual translation observation covariance matrix and the visual rotation observation covariance matrix. During training, the covariance matrix is ​​substituted into the Kalman filter fusion process, and the pose error between the fused trajectory and the reference trajectory is used as a constraint to optimize the model parameters end-to-end.

[0012] Step 5. Input the feature vector into the trained KAN covariance estimation model to obtain the visual translation observation covariance matrix and the visual rotation observation covariance matrix. Use the covariance matrix as the Kalman filter observation noise covariance to fuse visual observation and inertial prediction, and output the UAV pose estimation result.

[0013] Furthermore, the visual translation observation is a three-dimensional translation vector between image frames at adjacent time points, and the visual rotation observation is a rotation matrix between image frames at adjacent time points.

[0014] Furthermore, the feature vector includes basic visual quality features, feature point grid spatial distribution features, and consistency features between visual observation and inertial prediction, which are used to characterize the reliability of visual observation from the perspectives of feature quantity, matching quality, spatial distribution, and cross-sensor consistency.

[0015] Furthermore, the KAN covariance estimation model includes a spline KAN layer and an output linear layer, which outputs the original parameters used to construct the visual translation observation covariance matrix and the visual rotation observation covariance matrix.

[0016] Furthermore, both the visual translation observation covariance matrix and the visual rotation observation covariance matrix are 3×3 symmetric positive definite matrices, and are constructed using lower triangular matrices to ensure that the covariance matrices meet the numerical stability requirements of Kalman filter observation updates.

[0017] Furthermore, the KAN covariance estimation model adopts an end-to-end training method without explicit covariance labels, substitutes the output visual observation covariance matrix into the Kalman filter fusion process, and optimizes the model parameters in reverse based on the pose error between the fused trajectory and the reference trajectory.

[0018] Compared with the prior art, the beneficial effects of the present invention include:

[0019] 1. This invention does not use traditional fixed visual observation noise parameters. Instead, it uses the KAN covariance estimation model to adaptively output the visual observation covariance based on the reliability of visual observations. This enables the visual observation weights in Kalman filtering to be dynamically adjusted according to the observation quality, thereby improving the accuracy and robustness of UAV pose estimation in complex environments.

[0020] 2. This invention enables independent calculation of the covariance of visual translation observations and the covariance of rotation observations, allowing translation and rotation observations to be adaptively adjusted according to their respective reliability, thereby enhancing the adaptability of visual-inertial fusion to the degradation of different types of observations.

[0021] 3. This invention introduces multi-dimensional features such as basic visual quality, spatial distribution of feature point grids, and visual inertial consistency, enabling the covariance estimation model to simultaneously perceive feature quantity, matching quality, spatial distribution, and cross-sensor consistency, thereby improving the completeness of visual observation reliability modeling.

[0022] 4. This invention adopts an end-to-end training method without explicit covariance labels, substitutes the visual observation covariance output by KAN into the Kalman filter fusion process, and optimizes the model parameters in reverse through the fusion trajectory error, so that the model training is directly aimed at the optimization of UAV fusion pose estimation error, while retaining the interpretability of the Kalman filter fusion framework. Attached Figure Description

[0023] Figure 1This is a schematic diagram of the overall process of the UAV pose estimation method based on KAN adaptive observation covariance adjustment proposed in this invention;

[0024] Figure 2 This is a schematic diagram illustrating the construction of visual observation reliability features proposed in this invention;

[0025] Figure 3 This is a schematic diagram of the training process of the KAN covariance estimation model proposed in this invention;

[0026] Figure 4 This is a schematic diagram of the KAN output visual observation covariance participating in the Kalman filter observation update proposed in this invention; Detailed Implementation

[0027] The specific embodiments of the present invention will be described below with reference to the accompanying drawings. It should be understood that the described embodiments are used to illustrate the technical solutions of the present invention and are not intended to limit the scope of protection of the present invention.

[0028] like Figure 1 As shown, the method of the present invention includes steps such as data acquisition, binocular vision front-end processing, KAN covariance estimation model training, model loading, inertial prediction, visual observation covariance estimation, Kalman filter observation update, and pose result output.

[0029] First, acquire the left and right eye image sequences from the UAV's binocular camera, obtain the angular velocity and linear acceleration data from the inertial measurement unit (IMU), and read the binocular camera's intrinsic parameters, binocular baseline parameters, and extrinsic parameter calibration information between the camera and the IMU. Synchronize the binocular image sequences and inertial measurement data based on the image timestamps and inertial measurement timestamps.

[0030] Then, ORB feature extraction is performed on the left and right eye images through a binocular vision front-end to obtain left and right eye feature points; effective binocular depth points are calculated through descriptor matching and binocular geometric relationships; and visual translation and rotation observations between adjacent image frames are obtained through feature matching between adjacent frames and pose optimization based on reprojection error. The visual translation observation at the k-th visual observation time is denoted as... , Let be a three-dimensional translation vector; let be the visual rotation observation at the k-th visual observation time. , It is a 3×3 rotation matrix.

[0031] like Figure 2As shown, during the operation of the binocular vision front-end, a feature vector is constructed to characterize the reliability of visual observations. This feature vector includes basic visual quality features, feature point grid spatial distribution features, and visual inertial consistency features. The basic visual quality feature at the k-th visual observation time is denoted as... The basic visual quality features include feature quantity features, matching and optimization quality features, and depth disparity statistical features. The feature quantity features include the number of feature points in the left eye, the number of feature points in the right eye, and the number of effective binocular depth points; the matching and optimization quality features include the number of bag-of-words matching points and the number of pose-optimized inliers; the depth disparity statistical features include average depth, depth standard deviation, average disparity, and disparity standard deviation.

[0032] Let the spatial distribution characteristics of the feature points grid at the k-th visual observation time be denoted as... To determine the spatial distribution characteristics of feature points within a grid, the image is divided into M×N two-dimensional grid regions, where M and N are preset positive integers based on the image resolution and feature point density. The number of feature points within each grid is counted, and this count is divided by the total number of feature points included in the current frame to obtain the normalized number of feature points for each grid. Simultaneously, the feature point grid distribution entropy is calculated based on the probability distribution of feature points within the grid, which characterizes the overall spatial uniformity of feature point distribution in the image plane. In this embodiment, the image is divided into 12×8 two-dimensional grid regions to obtain the normalized number of feature points.

[0033] Let the visual inertial consistency feature at the k-th visual observation moment be denoted as... To assess the consistency between visual observations and inertial predictions, the inertial measurement unit data within the time interval between two adjacent visual frames is integrated for prediction, yielding the inertial prediction translation. and inertial prediction rotation The binocular visual front end obtains visual translation observations within the same time interval. and visual rotation observation ,according to and , and The differences between visual observations and inertial predictions are used to construct consistency features between them. These consistency features include translational consistency features, motion scale features, and rotational consistency features. Among these, translational consistency features include visual translational observations. Translation with inertial prediction The three-axis differences, three-axis absolute differences, and translation difference norms are considered. Motion scale characteristics include the velocity difference norm, inertial predicted translation norm, visual translation observation norm, and the ratio of the visual translation observation norm to the inertial predicted translation norm; rotational consistency characteristics include visual rotation observation... With inertial prediction rotation The rotation angle error and the difference norm of the rotation matrix.

[0034] Basic visual quality characteristics Feature point grid spatial distribution characteristics and visual inertial consistency characteristics The features are concatenated to form the input feature vector of the KAN covariance estimation model at the current time. ,Right now: The aforementioned basic visual quality features, feature point grid spatial distribution features, and visual inertial consistency features are used together to characterize the reliability of the current visual observation.

[0035] like Figure 3 As shown, a KAN covariance estimation model is constructed, wherein the model takes the input feature vector as input. The input parameters are processed through a feature normalization layer, a spline KAN layer, a nonlinear activation layer, and an output linear layer. The output parameters are used to construct the visual translation observation covariance matrix and the visual rotation observation covariance matrix. In this embodiment, the model includes two spline KAN layers and one output linear layer. Each spline KAN layer performs a nonlinear mapping on the input features through a learnable spline function, enabling the model to learn the nonlinear relationship between visual observation reliability features and observation uncertainty.

[0036] The output linear layer consists of two branches: one branch outputs the six raw parameters corresponding to visual translation observations, and the other branch outputs the six raw parameters corresponding to visual rotation observations. For any 3×3 covariance matrix, let the six raw parameters output by the model be... , , , , , ,Will , , By mapping positive values ​​and constraining them within a preset range, the diagonal elements of the lower triangular matrix are obtained. , , ,Will , , Through bounded nonlinear mapping, the off-diagonal elements of the lower triangular matrix are obtained. , , Thus, the lower triangular matrix L is constructed:

[0037]

[0038] For visual translation observations, construct the visual translation observation covariance matrix:

[0039]

[0040] For visual rotation observations, construct the visual rotation observation covariance matrix:

[0041]

[0042] in, This represents the visual translation observation covariance matrix at the k-th visual observation time. Let represent the visual rotation observation covariance matrix at the k-th visual observation time. This represents the baseline value for translation observation noise. This represents the reference value for rotating observation noise; and They represent the components used for construction. and The lower triangular matrix. By multiplying the lower triangular matrix with its transpose, the output observation covariance matrix can be guaranteed to be a symmetric positive definite matrix. For visual rotation observations, this invention does not directly establish the covariance of the nine elements of the rotation matrix, but describes the rotation observation error in the local tangent space of the three-dimensional rotation group SO(3) with a three-dimensional small-angle error vector. Therefore It is a 3×3 covariance matrix.

[0043] During model training, manual labeling of visual observation covariance is not required; the output of the KAN covariance estimation model is used. and Substituting into the Kalman filter fusion process, state prediction and observation update are carried out within a training window consisting of several consecutive visual observation times to obtain the fused trajectory within the window. Pose error constraints are constructed based on the position error term and attitude error term between the fused trajectory and the reference trajectory. At the same time, the training loss is formed by combining the covariance magnitude regularization term and the covariance smoothing term of adjacent time times. The parameters of the KAN covariance estimation model are optimized through backpropagation.

[0044] like Figure 4 As shown, in the pose estimation process, the trained KAN covariance estimation model is integrated into the Kalman filter fusion framework. The binocular vision front end provides visual translation observations at the k-th visual observation time. and visual rotation observation The inertial measurement unit (IMU) performs state prediction within the time interval between adjacent visual frames, obtaining the predicted pose and the corresponding prediction error covariance. Simultaneously, it extracts the feature vector from the current visual observation moment. Input the KAN covariance estimation model to obtain the visual translation observation covariance matrix. and visual rotation observation covariance matrix .

[0045] In Kalman filter observation update, the position error corresponding to visual translation observation is a three-dimensional vector, and the visual rotation observation, after being represented by the SO(3) local tangent space, forms a three-dimensional small-angle error vector. Therefore, the dimensions of the two are consistent with the dimensions of the position error state and attitude error state to be updated, respectively. In this embodiment, the observation matrix is ​​taken as the identity matrix. The position update gain and attitude update gain at the k-th visual observation time are calculated in the following forms:

[0046]

[0047]

[0048] in, This represents the covariance of the position prediction error at the k-th visual observation time. This represents the predicted covariance of the small-angle attitude error at the k-th visual observation time. This represents the visual translation observation covariance matrix output by the KAN covariance estimation model. This represents the visual rotation observation covariance matrix output by the KAN covariance estimation model. and Let represent the position update gain and the pose update gain, respectively. Further, based on the aforementioned position update gain and pose update gain, visual translation observations and visual rotation observations are used to correct the prior state obtained in the prediction stage, thus obtaining the posterior pose and posterior state error covariance at the k-th visual observation time.

[0049] When the observation covariance output by KAN is large, it indicates that the uncertainty of the current visual observation is high. The Kalman gain decreases, and the filter reduces the weight of the visual observation in the fusion update. Conversely, when the observation covariance output by KAN is small, it indicates that the current visual observation is relatively reliable. The Kalman gain increases, and the filter increases the weight of the visual observation in the fusion update. Therefore, the filter can dynamically adjust the fusion weights between visual observation and inertial prediction based on changes in the reliability of the visual observation.

[0050] according to Figure 1 The process shown repeats the feature input, KAN covariance prediction, inertial state prediction, Kalman gain calculation, and observation update at each visual observation moment, continuously outputting the pose estimation results of the UAV; when the pose estimation process ends, the recursive calculation stops.

[0051] In summary, this invention inputs the basic visual quality features, feature point grid spatial distribution features, and visual inertial consistency features extracted by the binocular vision front end into the KAN covariance estimation model, adaptively generating visual translation observation covariance matrices and visual rotation observation covariance matrices, which are then used as the Kalman filter observation noise covariance for observation updates. This method can dynamically adjust the visual observation weights according to changes in visual observation reliability, thereby improving the problem of insufficient adaptability of fixed observation noise parameters in complex UAV scenarios.

Claims

1. A method for UAV pose estimation based on KAN adaptive observation covariance adjustment, characterized in that, Includes the following steps: Step 1. Acquire the left and right eye image sequences captured by the UAV's binocular camera, the angular velocity and linear acceleration data collected by the inertial measurement unit, as well as the binocular camera's intrinsic parameters, binocular baseline parameters, and extrinsic parameter calibration information between the camera and the inertial measurement unit; Step 2. Perform ORB feature extraction, descriptor matching, and binocular depth calculation on the left and right eye images through a binocular vision front end. And obtain visual translation observation and visual rotation observation between adjacent frames through adjacent frame feature matching and pose optimization. Step 3. Extract features that characterize the reliability of visual observation during the operation of the binocular vision front end, and construct a feature vector, including basic visual quality features, feature point grid spatial distribution features, and consistency features between visual observation and inertial prediction; Step 4. Construct and train the KAN covariance estimation model. The model takes the feature vector as input and outputs the visual translation observation covariance matrix and the visual rotation observation covariance matrix. During training, the covariance matrix is ​​substituted into the Kalman filter fusion process, and the pose error between the fused trajectory and the reference trajectory is used as a constraint to optimize the model parameters end-to-end. Step 5. Input the feature vector into the trained KAN covariance estimation model to obtain the visual translation observation covariance matrix and the visual rotation observation covariance matrix. Use the covariance matrix as the Kalman filter observation noise covariance to fuse visual observation and inertial prediction, and output the UAV pose estimation result.

2. The UAV pose estimation method based on KAN adaptive observation covariance adjustment according to claim 1, characterized in that: The visual translation observation is a three-dimensional translation vector between adjacent image frames, and the visual rotation observation is a rotation matrix between adjacent image frames. The binocular vision front end obtains effective depth points based on the binocular geometric relationship between the left and right eye images, and performs feature matching based on bag-of-words matching and pose optimization based on reprojection error on adjacent image frames to obtain the three-dimensional translation vector and rotation matrix.

3. The UAV pose estimation method based on KAN adaptive observation covariance adjustment according to claim 1, characterized in that: The basic visual quality characteristics It includes feature quantity features, matching and optimization quality features, and depth disparity statistical features. The feature quantity features include the number of left-eye feature points, the number of right-eye feature points, and the number of effective binocular depth points; the matching and optimization quality features include the number of bag-of-words matches and the number of pose-optimized inliers; the depth disparity statistical features include average depth, depth standard deviation, average disparity, and disparity standard deviation.

4. The UAV pose estimation method based on KAN adaptive observation covariance adjustment according to claim 1, characterized in that: The feature point grid spatial distribution characteristics include the feature point grid distribution entropy and the normalized number of feature points within an M×N two-dimensional grid, where M and N are positive integers. The feature point grid distribution entropy is used to characterize the overall spatial uniformity of feature points in the image plane; the normalized number of feature points is obtained by dividing the image into an M×N two-dimensional grid, counting the number of feature points in each grid, and dividing the number of feature points in each grid by the total number of feature points participating in the statistics in the current frame, and is used to characterize the local distribution density of feature points in each grid region.

5. The UAV pose estimation method based on KAN adaptive observation covariance adjustment according to claim 1, characterized in that: The consistency characteristics between visual observation and inertial prediction include translational consistency characteristics, motion scale characteristics, and rotational consistency characteristics. Translational consistency characteristics include the three-axis difference, the absolute difference of the three axes, and the norm of the translation difference between visual and inertial predicted translations. Motion scale characteristics include the velocity difference norm, the norm of the inertial predicted translation, the norm of the visual translation, and the norm ratio between the visual and inertial predicted translations. Rotational consistency characteristics include the rotation angle error and the rotation matrix difference norm between visual and inertial predicted rotations.

6. The UAV pose estimation method based on KAN adaptive observation covariance adjustment according to claim 1, characterized in that: The KAN covariance estimation model includes at least one spline KAN layer and an output linear layer. The spline KAN layer constrains the layer input features within a preset range and constructs piecewise linear spline basis functions based on preset nodes. The spline basis function outputs are weighted and summed using learnable coefficients to obtain the spline branch result. Simultaneously, the linear branch performs a linear transformation on the layer input features. The spline branch result and the linear branch result are added together and then processed by a nonlinear activation function to obtain the latent features. The output linear layer outputs the original parameters used to construct the visual translation observation covariance matrix and the visual rotation observation covariance matrix based on the latent features.

7. The UAV pose estimation method based on KAN adaptive observation covariance adjustment according to claim 1, characterized in that: Both the visual translation observation covariance matrix and the visual rotation observation covariance matrix are 3×3 symmetric positive definite matrices. For any of these 3×3 symmetric positive definite matrices, the KAN covariance estimation model outputs six raw parameters. , , , , , ,Will , , By applying positive value mappings and constraining them within a preset range, the diagonal elements of the lower triangular matrix are obtained. , , ,Will , , The off-diagonal elements of the lower triangular matrix are obtained by bounded nonlinear mapping. , , And construct the lower triangular matrix L: The covariance matrix is ​​derived from Received, among which This represents the fixed observation noise baseline value corresponding to the observation covariance matrix. This represents the transpose of L.

8. The UAV pose estimation method based on KAN adaptive observation covariance adjustment according to claim 1, characterized in that: In the Kalman filter fusion process, the position error corresponding to the visual translation observation is a three-dimensional vector, and the visual rotation observation is represented by the SO(3) local tangent space to form a three-dimensional small-angle error vector. The two are consistent with the dimensions of the position error state and attitude error state to be updated, respectively. The observation matrix is ​​taken as the identity matrix, and the position update gain and attitude update gain are calculated according to the following formulas: in, This represents the covariance of the location prediction error. This represents the predicted covariance for small-angle attitude errors. This represents the visual translation observation covariance matrix output by the KAN covariance estimation model. This represents the small-angle error covariance matrix of the visual rotation observations output by the KAN covariance estimation model in the local tangent space of SO(3). Indicates position update gain. This represents the attitude update gain.

9. The UAV pose estimation method based on KAN adaptive observation covariance adjustment according to claim 1, characterized in that: The KAN covariance estimation model is trained in an end-to-end manner without explicit covariance labels. During model training, the visual observation covariance matrix output by the KAN covariance estimation model is substituted into the Kalman filter fusion process. Pose error constraints are constructed based on the position error and attitude error terms between the fused trajectory and the reference trajectory. The training loss is formed by combining the covariance magnitude regularization term and the covariance smoothing term between adjacent time steps, and the parameters of the KAN covariance estimation model are optimized in reverse.

Images