A visual positioning scale compensation kalman filtering method

Abstract

The application provides a visual positioning scale compensation Kalman filtering method, which realizes online real-time estimation and dynamic compensation of scale error by fusing height information, kinematic constraint and visual observation through explicitly taking scale error factor as a state variable in a state space model of extended Kalman filtering (EKF). The technical scheme of the application is applied to solve the technical problems of unstable scale estimation and large accumulated error of pose in the prior art visual inertial odometer (VIO) system when facing severe motion, visual degradation or low-cost IMU zero bias.

Description

Technical Field

[0001] This invention relates to the fields of visual inertial odometry (VIO) and autonomous navigation technology, and in particular to a Kalman filtering method for visual positioning scale compensation. Background Technology

[0002] Visual inertial odometry (VIO), by fusing data from cameras and inertial measurement units (IMUs), can reconstruct the pose of a vehicle and has become one of the core technologies for autonomous navigation in fields such as drones and robots. However, in practical applications, especially when drones are performing high-maneuver flights or in scenarios with sparse visual features and violent motion, traditional VIO systems still face challenges. IMU zero-bias drift and mismatches in the vision module can jointly lead to cumulative errors in pose estimation, seriously affecting the long-term accuracy and reliability of the system.

[0003] In existing technologies, common approaches to improving the robustness of VIO (Vehicle Identification and Positioning) include relying on high-precision IMU hardware or tightly coupling with a Global Navigation Satellite System (GNSS). However, the former significantly increases system cost and power consumption, making it unsuitable for consumer-grade or micro-UAV platforms; the latter will completely fail in environments where GNSS signals are denied, such as indoors or in canyons. Therefore, with limited sensor configurations, how to fully utilize the platform's own motion characteristics and other auxiliary information to provide a stable and independent scale factor for the VIO system, thereby enhancing its anti-interference capability and pose estimation accuracy, has become a critical problem that urgently needs to be solved. Summary of the Invention

[0004] This invention provides a Kalman filtering method for visual positioning scale compensation, which can solve the technical problems of unstable scale estimation and large cumulative pose error in existing visual inertial odometry (VIO) systems when faced with violent motion, visual degradation or the influence of zero bias of low-cost IMUs.

[0005] According to one aspect of the present invention, a Kalman filtering method for visual localization scale compensation is provided, the method comprising:

[0006] Establish a 16-dimensional state vector that includes position, velocity, attitude, IMU bias, and scale error factor;

[0007] A state equation based on IMU mechanical orchestration is constructed based on a 16-dimensional state vector, and the scaling error factor is modeled as a first-order Markov process.

[0008] Continuous state prediction is performed using IMU data to obtain state prediction values;

[0009] Each time a new image frame arrives, the visual inertial odometry extracts and matches feature points to obtain the current raw pixel displacement vector;

[0010] The drone's motion pattern is identified in real time based on IMU data and state prediction values. The original pixel displacement vector is filtered based on the drone's motion pattern, and outliers are removed to obtain the filtered and removed pixel displacement vectors.

[0011] The filtered and discarded pixel displacement vectors are combined with information from the height sensor, and the true displacement observation value is calculated using scale information.

[0012] A pixel displacement observation matrix is ​​established based on the filtered and removed pixel displacement vectors, a scale error observation matrix is ​​constructed using scale information, and a true displacement observation matrix is ​​established using the true displacement observation values.

[0013] An extended Kalman filter is performed based on the state equation, pixel displacement observation matrix, scale error observation matrix, and true displacement observation matrix to obtain the correction value of the scale error factor.

[0014] The true displacement output by the visual inertial odometry system is dynamically compensated by using the correction value of the scale error factor to obtain the compensated true displacement.

[0015] Furthermore, the 16-dimensional state vector is:

[0016]

[0017] In the above formula, x k Let denot be a k-dimensional state vector, k = 16, p, v, Φ, ε, ▽ represent the UAV's position, velocity, attitude, accelerometer bias, gyroscope bias, and so on, respectively, and δλ is the visual inertial scale error factor.

[0018] Furthermore, the state equation is:

[0019]

[0020] In the above formula, x represents the state variable, u is the raw measurement value of the IMU input to the system, including the specific force a measured by the accelerometer and the angular velocity ω measured by the gyroscope, w is the system process noise, which follows a zero-mean Gaussian distribution, and R wb (Φ) is the rotation matrix from the world frame to the machine frame, calculated from the attitude angle Φ, ε is the accelerometer zero bias, and n a For force noise, g is the gravity vector, E(Φ) is the transformation matrix from the body angular velocity to the Euler angle rate of change, and ω is the angular velocity measured by the gyroscope. For zero bias of the gyroscope, n ω For gyroscope noise, n ε To achieve zero bias noise in the accelerometer, For gyroscope zero-bias noise, β > 0 is the reciprocal of the correlation time constant, n λTo drive noise.

[0021] Furthermore, real-time identification of drone movement patterns based on IMU data and state prediction values ​​includes:

[0022] When the accelerometer reading magnitude is greater than the first preset threshold and the absolute values ​​of the differences between the Z-axis and X-axis components and the differences between the Z-axis and Y-axis components of the accelerometer reading are both greater than the second preset threshold, it is determined to be vertical motion.

[0023] When the Z-axis magnitude of the accelerometer reading remains below the third preset threshold, it is determined to be horizontal uniform motion.

[0024] Furthermore, the original pixel displacement vectors are filtered based on the drone's motion pattern, and outliers are removed, including:

[0025] The theoretical direction of the pixel displacement vector is determined based on the drone's motion pattern, and the original pixel displacement vectors that do not conform to the theoretical direction are eliminated.

[0026] The predicted pixel displacement vector is calculated based on the drone's motion pattern;

[0027] Calculate the absolute value of the difference between the predicted pixel displacement vector and the original pixel displacement, compare it with the fourth preset threshold, and discard the original pixel displacement corresponding to the absolute value of the difference that is greater than the fourth preset threshold.

[0028] Furthermore, the true displacement output by the visual inertial odometry system is dynamically compensated using the correction value of the scale error factor through the following formula:

[0029] P corrected =(1+δλ) k )P VIO

[0030] In the above formula, P corrected To calculate the actual displacement after compensation, P VIO This represents the actual displacement output by the inertial visual odometry.

[0031] This invention provides a Kalman filter method for visual positioning scale compensation. This method explicitly incorporates the scale error factor as a state variable into the state-space model of the Extended Kalman Filter (EKF). By fusing altitude information, kinematic constraints, and visual observations, it achieves online real-time estimation and dynamic compensation of scale errors. Specifically, based on the basic VIO framework, it uses the UAV kinematic model as a strong constraint on the pixel displacement vector. Through motion pattern recognition and abnormal displacement removal mechanisms, it significantly improves the reliability of feature point tracking. Furthermore, it dynamically compensates and corrects the scale of visual observations by explicitly modeling and estimating a time-varying scale error factor online. This method can improve pixel displacement quality through kinematic constraints under low-precision IMU conditions, effectively suppressing VIO scale drift and error accumulation, and significantly improving the positioning accuracy and system robustness of UAVs in complex scenarios with satellite denial. Attached Figure Description

[0032] The accompanying drawings, which form part of this specification, are provided to further illustrate embodiments of the invention and, together with the textual description, explain the principles of the invention. It is obvious that the drawings described below are merely some embodiments of the invention, and those skilled in the art can obtain other drawings based on these drawings without any creative effort.

[0033] Figure 1 A schematic flowchart of a visual positioning scale compensation Kalman filtering method according to a specific embodiment of the present invention is shown. Detailed Implementation

[0034] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. 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 a part of the embodiments of the present invention, and not all of them. The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit the present invention or its application or use. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0035] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this application. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0036] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps set forth in these embodiments do not limit the scope of the invention. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following figures denote similar items; therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0037] like Figure 1 As shown, a Kalman filtering method for visual positioning scale compensation is provided according to a specific embodiment of the present invention, the method comprising:

[0038] S1, establish a 16-dimensional state vector containing position, velocity, attitude, IMU bias, and scale error factor;

[0039] S2, based on the 16-dimensional state vector, constructs the state equation based on the mechanical arrangement of the IMU, and models the scale error factor as a first-order Markov process;

[0040] S3, continuously perform state prediction using IMU data to obtain state prediction values;

[0041] S4, each time a new image frame arrives, the visual inertial odometry extracts and matches feature points to obtain the current original pixel displacement vector;

[0042] S5 identifies the drone's motion mode in real time based on IMU data and state prediction values, filters the original pixel displacement vectors based on the drone's motion mode, and removes outliers to obtain the filtered and removed pixel displacement vectors.

[0043] S6 combines the filtered and discarded pixel displacement vectors with the information from the height sensor, and calculates the true displacement observation value through scale information;

[0044] S7. Based on the filtered and removed pixel displacement vectors, establish a pixel displacement observation matrix, construct a scale error observation matrix using scale information, and establish a true displacement observation matrix using the true displacement observation values.

[0045] S8. Based on the state equation, pixel displacement observation matrix, scale error observation matrix and true displacement observation matrix, extended Kalman filtering is performed to obtain the correction value of scale error factor;

[0046] S9 uses the correction value of the scale error factor to dynamically compensate the true displacement output by the visual inertial odometry system, and obtains the compensated true displacement.

[0047] This configuration provides a Kalman filter method for visual positioning scale compensation. This method explicitly incorporates the scale error factor as a state variable into the state-space model of the Extended Kalman Filter (EKF). By fusing altitude information, kinematic constraints, and visual observations, it achieves online real-time estimation and dynamic compensation of scale errors. Specifically, based on the basic VIO framework, it uses the UAV kinematic model as a strong constraint on the pixel displacement vector. Through motion pattern recognition and abnormal displacement removal mechanisms, it significantly improves the reliability of feature point tracking. Furthermore, it dynamically compensates and corrects the scale of visual observations by explicitly modeling and estimating a time-varying scale error factor online. This method can improve pixel displacement quality through kinematic constraints under low-precision IMU conditions, effectively suppressing VIO scale drift and error accumulation, and significantly improving the positioning accuracy and system robustness of UAVs in complex scenarios with satellite denial. Compared with existing technologies, the technical solution of this invention can solve the technical problems of unstable scale estimation and large pose accumulation errors in existing visual inertial odometry (VIO) systems when facing severe motion, visual degradation, or the influence of zero bias from low-cost IMUs.

[0048] Furthermore, in this embodiment of the invention, the 16-dimensional state vector is:

[0049]

[0050] In the above formula, x k Let p, v, Φ, ε represent a k-dimensional state vector, k = 16. The values ​​represent the UAV's position, velocity, attitude, accelerometer bias, and gyroscope bias, respectively, with δλ representing the visual inertial scale error factor.

[0051] Based on the above embodiments, in this embodiment of the invention, the state equation is:

[0052]

[0053]

[0054] In the above formula, x represents the state variable, u is the raw measurement value of the IMU input to the system, including the specific force a measured by the accelerometer and the angular velocity ω measured by the gyroscope, w is the system process noise, which follows a zero-mean Gaussian distribution, and R wb (Φ) is the rotation matrix from the world frame to the machine frame, calculated from the attitude angle Φ, ε is the accelerometer zero bias, and n a For force noise, g is the gravity vector, E(Φ) is the transformation matrix from the body angular velocity to the Euler angle rate of change, and ω is the angular velocity measured by the gyroscope. For zero bias of the gyroscope, n ω For gyroscope noise, n ε For zero bias noise of the accelerometer, n ▽ For gyroscope zero-bias noise, β > 0 is the reciprocal of the correlation time constant, n λ To drive noise.

[0055] Furthermore, in this embodiment of the invention, the real-time identification of the UAV's motion mode based on IMU data and state prediction values ​​includes: when the accelerometer reading magnitude is greater than a first preset threshold and the absolute values ​​of the differences between the Z-axis and X-axis components of the accelerometer reading, as well as the absolute values ​​of the differences between the Z-axis and Y-axis components, are both greater than a second preset threshold, it is determined to be vertical motion; when the accelerometer reading Z-axis magnitude is continuously lower than a third preset threshold, it is determined to be horizontal uniform motion.

[0056] Based on the above embodiments, in this embodiment of the invention, filtering the original pixel displacement vectors based on the drone motion mode and removing outliers includes: determining the theoretical direction of the pixel displacement vector according to the drone motion mode, and removing original pixel displacement vectors that do not conform to the theoretical direction; calculating the predicted pixel displacement vector according to the drone motion mode; calculating the absolute value of the difference between the predicted pixel displacement vector and the original pixel displacement, and comparing it with a fourth preset threshold, removing the original pixel displacements corresponding to the absolute value of the difference greater than the fourth preset threshold. The specific values ​​of the first preset threshold, the second preset threshold, the third preset threshold, and the fourth preset threshold are determined according to the actual situation.

[0057] Furthermore, in this embodiment of the invention, the true displacement output by the visual inertial odometry system is dynamically compensated using the correction value of the scale error factor through the following formula:

[0058] P corrected =(1+δλ) k )P VIO

[0059] In the above formula, P corrected To calculate the actual displacement after compensation, P VIO This represents the actual displacement output by the inertial visual odometry.

[0060] To facilitate a clearer understanding of the visual positioning scale compensation Kalman filtering method provided by this invention, the following will use practical application examples to illustrate the above processes in detail. Those skilled in the art will understand that this example is only for the purpose of facilitating a clearer understanding of the visual positioning scale compensation Kalman filtering method provided by this invention, and does not impose any technical limitations on it.

[0061] The overall process of the visual positioning scale compensation Kalman filter method of this invention can be summarized as follows:

[0062] 1) First, system initialization is performed: a 16-dimensional state vector is defined, including position, velocity, attitude, IMU bias, and the key innovative state variable—the scale error factor δλ. Each state value is initialized, where position can be set to the origin of the world coordinate system, velocity is zero, attitude is determined by initial sensor readings, and the IMU bias is initialized to zero. Simultaneously, the error covariance matrix is ​​initialized, and a large initial variance is assigned to δλ to reflect its initial uncertainty. A state equation based on IMU mechanical orchestration is constructed, and the scale error factor δλ is modeled as a first-order Markov process to describe its slow time-varying characteristics.

[0063] 2) In real-time operation, multi-source observation data is processed and EKF fusion is performed: IMU data is continuously used for state prediction; when a new image frame arrives, the visual front end performs feature point extraction and matching to obtain the original pixel displacement vector Δu. meas Then, based on IMU data and state prediction values, the drone's motion mode (such as vertical or constant speed) is identified in real time, and this kinematic model is used to analyze Δu. meas By filtering and removing outliers whose direction or size does not conform to theoretical expectations, a clean pixel displacement vector Δu is obtained. constrained ; then, Δu constrained By combining height sensor information with scale information, the actual displacement observation value z is calculated. scale Finally, in the EKF update step, pixel displacement constraint observations, scale observations, and true displacement error observations are sequentially incorporated to optimally estimate and update all states, especially to accurately correct the scale error factor δλ;

[0064] 3) Achieve real-time scale compensation and reliable output: After each frame of EKF update, the displacement output by the visual inertial odometry system is dynamically compensated using the latest estimated scale error factor δλ to obtain the scale-corrected displacement with the correct physical scale; finally, the system outputs P. corrected This serves as the result of high-precision navigation for unmanned aerial vehicles (UAVs). The process is executed cyclically to achieve online real-time estimation and compensation of scale errors.

[0065] Specifically, the overall architecture of this method is a 16-dimensional state vector that includes a scaling error factor, defined as follows:

[0066]

[0067] Where p,v,Φ,ε, The values ​​represent the UAV's position, velocity, attitude, accelerometer bias, and gyroscope bias, respectively. δλ is the visual inertial scale error factor, used to characterize and compensate for the comprehensive scale error caused by visual depth uncertainty, IMU bias drift, altitude error, sensor calibration error, etc. in a monocular VIO system.

[0068] (1) State equations (system dynamics model)

[0069] The continuous-time nonlinear state equation (dynamic model) of the system is as follows:

[0070]

[0071] Where x is the state variable, u is the system input, i.e. the raw measurement value of the IMU, including the specific force a measured by the accelerometer and the angular velocity ω measured by the gyroscope; w is the system process noise, which follows a zero-mean Gaussian distribution.

[0072] The differential equations for each state variable are as follows:

[0073] Position dynamics:

[0074] Velocity dynamics: Where R wb (Φ) is the rotation matrix from the world frame to the machine frame, calculated from the attitude angle Φ, where a is the specific force, ε is the accelerometer zero bias, and n a For force noise, g is the gravity vector;

[0075] Attitude dynamics: Where E(Φ) is the transformation matrix from the body angular velocity to the Euler angle rate of change, and ω is the angular velocity measured by the gyroscope. For zero bias of the gyroscope, n ω This is gyroscope noise;

[0076] Zero-biased dynamics: n ε To achieve zero bias noise in the accelerometer, The zero-bias noise of the gyroscope is modeled as a random walk process.

[0077] Scale error factor dynamics: The scaling error factor δλ is modeled as a first-order Markov process, where β > 0 is the reciprocal of the correlation time constant (default is 0.02), and n λTo drive the noise, this model is used to track the slow time-varying characteristics of the scaling error.

[0078] (2) Pixel displacement constraints based on kinematic model

[0079] By using UAV kinematic models to predict and constrain the pixel displacement vector Δu of image feature points, more reliable observations can be provided for scale estimation.

[0080] a) Kinematic prediction of pixel displacement vector

[0081] Given an approximate motion pattern of the drone, the direction and magnitude of the displacement of feature point pixels can be predicted. For example:

[0082] Vertical motion: When a drone takes off and lands purely vertically, the pixel displacement vector Δu of its imaging of ground feature points should theoretically diverge radially or converge at the vanishing point in the image (usually close to the image center).

[0083] Horizontal uniform motion: When the UAV flies horizontally at a constant speed in a straight line, the pixel displacement vector Δu of the ground feature points should have a common direction (i.e., focus on the vanishing point in front).

[0084] The predicted pixel displacement vector is denoted as Δu. pred .

[0085] b) Observation equation for pixel displacement

[0086] The difference between the kinematically predicted pixel displacement and the visually measured pixel displacement is used as the observation:

[0087] z pixel =Δu meas -Δu pred (v,Φ)+ν pixel

[0088] Where, Δu meas ν is the pixel displacement actually measured visually. pixel For pixel error noise, Δu pred It is a function of velocity v and attitude Φ, expressed as:

[0089]

[0090] Where K is the camera intrinsic parameter, Δt is the rotation matrix from the world coordinate system to the camera coordinate system, v is the estimated velocity of the UAV, and Δt is the image sampling period.

[0091] The residuals of this observation reflect the error in the estimation of the motion state, including information about the scale error.

[0092] c) Observation equations of the true displacement

[0093] The actual displacement observation model is:

[0094] z scale =(s0+δλ)×Δu constrained +ν scale

[0095] The nominal scale factor s0 is calculated from the relative height h, pixel size d, and camera focal length f: s0 = (h·d) / f. The pixel displacement vector Δu, after kinematic constraint filtering and correction... constrained It was used to calculate more reliable z scale This directly optimizes the estimation of δλ.

[0096] (3) Multi-source observation

[0097] This method constructs an EKF framework that includes three types of observations:

[0098] Pixel displacement constraint observation: As mentioned above, it provides indirect constraints on motion state and scale;

[0099] Pixel-scale observation: Provides direct observation of δλ based on height and camera intrinsic parameters;

[0100] True displacement error observation: Based on scale and pixel displacement, directly observe the true displacement error of inertia and vision.

[0101] The scale-compensated extended Kalman filter algorithm is as follows:

[0102] (1) State prediction

[0103] x k|k-1 =f(x) k-1 ,u)

[0104]

[0105] Where, x k|k-1 For state prediction in one step, x k-1 Let P be the system state at time k-1, u be the system input, and P be the system state at time k-1. k|k-1 To predict the mean squared error matrix, F k Let P be the state transition matrix. k-1 For, Q k This is state transition noise.

[0106] (2) Measurement update

[0107] First, based on IMU data and state prediction, identify the current flight mode (e.g., vertical, horizontal constant speed, etc.):

[0108] a) Vertical motion detection: When the accelerometer reading modulus exceeds the threshold of 5 m / s 2Furthermore, when the Z-axis component of the accelerometer reading is significantly greater than that of the X and Y axes, it is determined to be vertical motion. In this case, the direction angle of the pixel displacement of all feature points in the image is calculated, and points that deviate too much from the radial direction (pointing towards the image center) are removed.

[0109] b) Horizontal uniform motion detection: When the Z-axis magnitude of the accelerometer reading remains below the threshold of 2 m / s 2 When the motion is determined to be horizontal and uniform, a statistical histogram of the pixel displacement direction is calculated, and outliers with directions that differ significantly from the main direction are removed.

[0110] Then, the predicted pixel displacement vector Δu is calculated based on the kinematic model. pred The actual measured pixel displacement vector (original pixel displacement vector) Δu meas With Δu pred The comparison process involves setting a threshold, removing outlier feature points with excessively large differences in direction or size, and finally utilizing the pixel displacement residual z that conforms to kinematic constraints. pixel It updates the velocity v, attitude Φ, and other states. Its observation matrix H pixel The pixel displacement and state variable are linked by a chain rule.

[0111] Using the more reliable pixel displacement vector Δu selected in the previous step constrained The actual displacement observation value z is calculated by combining the height h. scale Therefore, the actual displacement error observed is:

[0112] Δz scale =z scale -z ins

[0113] Among them, z ins These are the displacement values ​​calculated for adjacent frames from the IMU. The actual displacement observation matrix is:

[0114] H t =[0 3×3 ,I 3×3 ,0 3×3 ,0 3×3 ,0 3×3 ,0 3×1 ]

[0115] Among them, I 3×3 It is a unit array.

[0116] The scale error observation matrix is:

[0117] H λ =[0,d / f,0,0,0,0,0,0]

[0118] Where d is the pixel size and f is the camera focal length.

[0119] (3) Real-time compensation for scale error

[0120] After each filter update, the output of VIO is compensated using the latest estimated δλ:

[0121] P corrected =(1+δλ) k )P VIO

[0122] Among them, P corrected To calculate the actual displacement after compensation, P VIO This represents the actual displacement output by the inertial visual odometry.

[0123] In summary, this invention provides a Kalman filter method for visual positioning scale compensation. This method explicitly incorporates the scale error factor as a state variable into the state-space model of the Extended Kalman Filter (EKF). By fusing altitude information, kinematic constraints, and visual observations, it achieves online real-time estimation and dynamic compensation of scale errors. Specifically, based on the basic VIO framework, it uses the UAV kinematic model as a strong constraint on the pixel displacement vector. Through motion pattern recognition and abnormal displacement removal mechanisms, it significantly improves the reliability of feature point tracking. Furthermore, it dynamically compensates and corrects the scale of visual observations by explicitly modeling and estimating a time-varying scale error factor online. This method can improve pixel displacement quality through kinematic constraints under low-precision IMU conditions, effectively suppressing VIO scale drift and error accumulation, and significantly improving the positioning accuracy and system robustness of UAVs in complex scenarios with satellite denial. Compared with existing technologies, the technical solution of this invention can solve the technical problems of unstable scale estimation and large pose accumulation errors in existing visual inertial odometry (VIO) systems when facing severe motion, visual degradation, or the influence of zero bias from low-cost IMUs.

[0124] For ease of description, spatial relative terms such as "above," "on top of," "on the upper surface of," "above," etc., are used herein to describe the spatial positional relationship of a device or feature as shown in the figures to other devices or features. It should be understood that spatial relative terms are intended to encompass different orientations in use or operation beyond the orientation of the device as described in the figures. For example, if the device in the figures were inverted, a device described as "above" or "on top of" other devices or structures would subsequently be positioned as "below" or "under" other devices or structures. Thus, the exemplary term "above" can include both "above" and "below." The device may also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatial relative descriptions used herein will be interpreted accordingly.

[0125] Furthermore, it should be noted that the use of terms such as "first" and "second" to define components is merely for the purpose of distinguishing the corresponding components. Unless otherwise stated, the above terms have no special meaning and therefore should not be construed as limiting the scope of protection of this invention.

[0126] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A Kalman filter method for visual localization scale compensation, characterized in that, The method includes: Establish a 16-dimensional state vector that includes position, velocity, attitude, IMU bias, and scale error factor; A state equation based on IMU mechanical orchestration is constructed based on a 16-dimensional state vector, and the scaling error factor is modeled as a first-order Markov process. Continuous state prediction is performed using IMU data to obtain state prediction values; Each time a new image frame arrives, the visual inertial odometry extracts and matches feature points to obtain the current raw pixel displacement vector; The drone's motion pattern is identified in real time based on IMU data and state prediction values. The original pixel displacement vector is filtered based on the drone's motion pattern, and outliers are removed to obtain the filtered and removed pixel displacement vectors. The filtered and discarded pixel displacement vectors are combined with information from the height sensor, and the true displacement observation value is calculated using scale information. A pixel displacement observation matrix is ​​established based on the filtered and removed pixel displacement vectors, a scale error observation matrix is ​​constructed using scale information, and a true displacement observation matrix is ​​established using the true displacement observation values. Based on the state equation, the pixel displacement observation matrix, the scale error observation matrix, and the true displacement observation matrix, an extended Kalman filter is performed to obtain the correction value of the scale error factor. The true displacement output by the visual inertial odometry system is dynamically compensated by the correction value of the scale error factor to obtain the compensated true displacement.

2. The method according to claim 1, characterized in that, The 16-dimensional state vector is: In the above formula, x k Let p, v, Φ, ε represent a k-dimensional state vector, k = 16. The values ​​represent the UAV's position, velocity, attitude, accelerometer bias, and gyroscope bias, respectively, with δλ representing the visual inertial scale error factor.

3. The method according to claim 1 or 2, characterized in that, The state equation is: In the above formula, x represents the state variable, u is the raw measurement value of the IMU input to the system, including the specific force a measured by the accelerometer and the angular velocity ω measured by the gyroscope, w is the system process noise, which follows a zero-mean Gaussian distribution, and R wb (Φ) is the rotation matrix from the world frame to the machine frame, calculated from the attitude angle Φ, ε is the accelerometer zero bias, and n a For force noise, g is the gravity vector, E(Φ) is the transformation matrix from the body angular velocity to the Euler angle rate of change, and ω is the angular velocity measured by the gyroscope. For zero bias of the gyroscope, n ω For gyroscope noise, n ε To achieve zero bias noise in the accelerometer, For gyroscope zero-bias noise, β > 0 is the reciprocal of the correlation time constant, n λ To drive noise.

4. The method according to claim 3, characterized in that, Real-time identification of drone movement patterns based on IMU data and state prediction values ​​includes: When the accelerometer reading magnitude is greater than the first preset threshold and the absolute values ​​of the differences between the Z-axis and X-axis components and the differences between the Z-axis and Y-axis components of the accelerometer reading are both greater than the second preset threshold, it is determined to be vertical motion. When the Z-axis magnitude of the accelerometer reading remains below the third preset threshold, it is determined to be horizontal uniform motion.

5. The method according to claim 4, characterized in that, The original pixel displacement vectors are filtered based on the drone's motion pattern, and outliers are removed, including: The theoretical direction of the pixel displacement vector is determined based on the drone's motion pattern, and the original pixel displacement vectors that do not conform to the theoretical direction are eliminated. The predicted pixel displacement vector is calculated based on the drone's motion pattern; Calculate the absolute value of the difference between the predicted pixel displacement vector and the original pixel displacement, compare it with the fourth preset threshold, and discard the original pixel displacement corresponding to the absolute value of the difference that is greater than the fourth preset threshold.

6. The method according to any one of claims 1 to 5, characterized in that, The following formula is used to dynamically compensate for the true displacement output by the visual inertial odometry system using the correction value of the scale error factor: P corrected =(1+sl k )P VIO In the above formula, P corrected For the actual displacement after compensation, P VIO This represents the actual displacement output by the inertial visual odometry.

Images