An adaptive filtering method for inertial and visual integrated navigation based on quality factor

Through the adaptive filtering method based on quality factors, the observation noise matrix is ​​adjusted to adapt to the visual matching quality, and the navigation accuracy reduction caused by unstable visual matching quality in drone navigation is solved, achieving higher navigation accuracy and stability.

CN118243094BActive Publication Date: 2024-11-12BEIJING AUTOMATION CONTROL EQUIP INST
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202410427612.2
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2024-04-10
Publication Date
2024-11-12
Estimated Expiration
2044-04-10

AI Technical Summary

Technical Problem

When the visual matching quality of existing drone inertial vision combination navigation decreases in navigation accuracy, and completely eliminating under-matching information will reduce the amount of available information, which cannot effectively suppress inertial navigation errors.

Method used

The inertial visual combination navigation adaptive filtering method based on quality factors is designed, and the observation noise matrix is ​​adaptively calculated, and the observation noise of the filter is adjusted according to the visual matching quality, so as to achieve flexible adaptation to the visual matching results of different qualities.

Benefits of technology

It significantly improves the adaptability and fusion accuracy of inertial vision combined navigation, improves the stability and reliability of navigation results, and meets the high-precision and high-reliability navigation needs.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118243094B_ABST
    Figure CN118243094B_ABST
Patent Text Reader

Abstract

The present invention provides an adaptive filtering method for inertial vision integrated navigation based on quality factor, including constructing an adaptive filtering state quantity and observation model for inertial vision integrated navigation; designing an adaptive calculation method for an observation noise matrix based on quality factor; performing Kalman adaptive filtering estimation based on the state quantity, observation model, and observation noise matrix; and adaptively compensating and correcting related errors based on the filtering estimation effect. The present invention adaptively adjusts the filter observation noise based on the visual matching quality factor, realizes flexible adaptation to image matching results of different qualities, and significantly improves the adaptability and fusion accuracy of inertial vision integrated navigation in different scenarios.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of integrated navigation, and in particular relates to an inertial vision integrated navigation adaptive filtering method based on quality factor. Background Art

[0002] Inertial visual integrated navigation combines the characteristics of autonomous, continuous, real-time inertial navigation and high precision of visual navigation. It is an effective means for UAVs and other carriers to achieve high-precision and strong autonomous navigation and positioning. The existing UAV inertial visual integrated navigation in the cruise phase generally takes the position information provided by visual navigation as the benchmark, and estimates and corrects the inertial navigation error through fusion filtering. In this process, the accuracy and reliability of visual navigation position information have a direct impact on the integrated navigation results. At present, in the fusion filtering process, the position information is simply judged as "available" or "unavailable" based on the visual matching situation, and the fusion filtering cannot be dynamically adjusted according to the image matching quality, resulting in the combined navigation results being significantly affected by the visual matching effect. Especially when the visual matching quality is unstable, the combined filtering using the visual position information with poor matching quality will lead to a significant decrease in the accuracy of the combined navigation, and completely eliminating the visual position information with poor matching quality will greatly reduce the amount of available information, and the inertial navigation error cannot be corrected enough times, and the inertial navigation error cannot be effectively suppressed. Summary of the invention

[0003] The present invention aims to solve one of the above-mentioned technical problems and provides an adaptive filtering method for inertial vision integrated navigation based on quality factor. The method adaptively adjusts the filter observation noise based on the visual matching quality factor, realizes flexible adaptation to image matching results of different qualities, and significantly improves the adaptability and fusion accuracy of inertial vision integrated navigation in different scenarios.

[0004] The technical solution adopted by the present invention to solve the above technical problems is as follows:

[0005] The present invention provides an inertial vision integrated navigation adaptive filtering method based on quality factor, comprising the following steps:

[0006] Construct the adaptive filtering state and observation model of inertial vision integrated navigation;

[0007] An adaptive calculation method for the observation noise matrix is ​​designed based on the quality factor. The calculation method for the observation noise matrix is:

[0008]

[0009] Among them, R k is the filter observation noise matrix, R1 is the set basic image observation noise matrix; R2 is the set matching observation noise matrix; τ1 is the image quality factor, τ2 is the matching quality factor;

[0010] Perform Kalman adaptive filtering estimation based on the state quantity, observation model, and observation noise matrix;

[0011] The correlation errors are adaptively compensated and corrected based on the filtering estimation effect.

[0012] Furthermore, the state quantity at least includes position error, speed error, misalignment angle, and inertial device error.

[0013] Furthermore, the state quantity includes the latitude error and longitude error of the inertial navigation, the north and east velocity errors of the inertial navigation, the north, celestial and east misalignment angles of the inertial navigation, the accelerometer zero bias in three orthogonal directions in the inertial navigation carrier coordinate system, and the gyroscope drift in three orthogonal directions in the inertial navigation carrier coordinate system.

[0014] Furthermore, the observation model includes observation quantities and an observation matrix;

[0015] The observed value is the difference between the inertial navigation result and the visual navigation result, expressed as:

[0016]

[0017] in, λ ins are the latitude and longitude calculated by inertial navigation respectively; λ vns are the latitude and longitude calculated by visual navigation respectively;

[0018] The observation matrix is ​​expressed as:

[0019]

[0020] Furthermore, the method for setting the basic image observation noise matrix is:

[0021]

[0022] Among them, σ r1 is the reference image noise;

[0023] The setting method of the matching observation noise matrix is:

[0024]

[0025] σ r2 The noise is from the actual shot.

[0026] Furthermore, σ r1 The value range is 50% to 150% of the reference image resolution. r2 The value range is 50% to 150% of the actual image resolution.

[0027] Furthermore, the calculation method of the image quality factor and the matching quality factor is as follows:

[0028]

[0029] Among them, A is the number of feature points contained in a preset frame of image, B is the number of feature point pairs with the same name obtained by visual feature matching between the real image and the reference image, and C is the number of inliers obtained by homography estimation based on the feature point pairs with the same name.

[0030] Furthermore, the visual feature matching between the real shot image and the reference image adopts a nearest neighbor method or a feature matching network method, and the number of inliers is obtained by homography estimation based on the feature point pairs with the same name using a random consistency sampling method.

[0031] The beneficial effects of the present invention compared with the prior art are as follows:

[0032] (1) The present invention designs an adaptive filtering model for inertial visual integrated navigation based on quality factor, and adaptively adjusts the utilization of visual matching results based on the visual matching quality factor. When the visual matching quality is good, its auxiliary effect is fully utilized to achieve high-precision integrated navigation; when the visual matching quality is poor, its utilization weight is reduced and the correction effect is maintained to maintain a high-precision combined effect. Through this method, the visual matching results of different qualities are dynamically adapted, the utilization rate of the visual matching results is greatly improved, and the accuracy of the integrated navigation throughout the whole process is significantly improved.

[0033] (2) The present invention uses an adaptive method to accurately adjust the weight of the visual matching results in the fusion filtering process, and can adapt to the needs of inertial vision combined navigation in different scenarios. In particular, in flight areas where visual features vary greatly and matching quality is unstable, the method of the present invention can significantly improve the stability and reliability of the inertial vision combined navigation results, meeting the high-precision and high-reliability navigation and positioning requirements of UAVs. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] The included drawings are used to provide a further understanding of the embodiments of the present invention, which constitute a part of the specification, are used to illustrate the embodiments of the present invention, and together with the text description, explain the principles of the present invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0035] Figure 1 A flow chart of adaptive filtering of inertial vision integrated navigation based on quality factor provided by a specific embodiment of the present invention;

[0036] Figure 2The change of visual matching quality factor in a test provided by a specific embodiment of the present invention;

[0037] Figure 3 The results of the adaptive filtering of the present invention and the conventional method are compared in an experiment provided for a specific embodiment of the present invention. DETAILED DESCRIPTION

[0038] Specific embodiments of the present invention are described in detail below. In the following description, for the purpose of explanation and not limitation, specific details are set forth to help fully understand the present invention. However, it will be apparent to those skilled in the art that the present invention may also be practiced in other embodiments that depart from these specific details.

[0039] It should be noted that in order to avoid obscuring the present invention due to unnecessary details, only the device structure and / or processing steps closely related to the scheme of the present invention are shown in the drawings, while other details that are not closely related to the present invention are omitted.

[0040] The present invention proposes an adaptive filtering method for inertial vision integrated navigation based on quality factor, constructs an adaptive filtering model for inertial vision integrated navigation, designs an adaptive filtering method based on quality factor, realizes adaptive filtering estimation based on quality factor, adaptively compensates and corrects related errors based on filtering estimation effect, and realizes high-precision integrated navigation under different quality visual matching results in complex scenes.

[0041] As a specific embodiment of the present invention, an adaptive filtering method for inertial vision integrated navigation based on quality factor is as follows: Figure 1 As shown, the implementation process is as follows:

[0042] (1) Construct an adaptive filtering state quantity that includes the main error factors of inertial and visual integrated navigation.

[0043] Filter state X k Including 13-dimensional state variables such as position error, velocity error, misalignment angle, inertial device error, etc., defined as:

[0044]

[0045] in, δλ are the latitude error and longitude error of inertial navigation respectively; δV n ,δV e are the north and east velocity errors of inertial navigation respectively; φ n ,φ u ,φ e They are the north, sky and east misalignment angles of inertial navigation respectively; are the accelerometer zero bias in the inertial navigation carrier coordinate system; εx , ε y , ε z are the gyro drifts in the inertial navigation carrier coordinate system respectively.

[0046] (2) Construct a high-precision inertial vision combined navigation observation model.

[0047] Observation quantity Z k is the difference between the inertial navigation result and the visual navigation result, defined as

[0048]

[0049] in, λ ins are the latitude and longitude calculated by inertial navigation respectively; λ vns They are the latitude and longitude calculated by visual navigation respectively.

[0050] The observation matrix H k Defined as:

[0051]

[0052] (3) Design an adaptive calculation method for observation noise based on quality factor.

[0053] R k is the filter observation noise matrix, defined as:

[0054]

[0055] Among them, R1 is the set basic image observation noise matrix; R2 is the set matching observation noise matrix; τ1 is the image quality factor, and τ2 is the matching quality factor.

[0056] The setting method of R1 and R2 is as follows:

[0057]

[0058] σ r1 is the reference image noise, which is related to the resolution of the visual matching reference image used, and generally ranges from 50% to 150% of the reference image resolution; σ r2 It is the noise of the real shot image, which is related to the resolution of the real shot image of the camera used. The general value range is 50% to 150% of the resolution of the real shot image. In different applications, the above setting value can be adjusted according to the application scenario.

[0059] In this embodiment,

[0060]

[0061] The quality factors τ1 and τ2 are calculated as follows:

[0062]

[0063] Among them, A is the number of feature points contained in a preset frame of image, B is the number of pairs of feature points with the same name obtained by matching the visual features of the real image with the reference image using the nearest neighbor method or feature matching network, and C is the number of inliers obtained by homography estimation based on the pairs of feature points with the same name using methods such as random consistency sampling. The image quality factor τ1 is used to evaluate the richness of the visual features in the image, and the matching quality factor τ2 is used to evaluate the degree of use of the visual features contained in the image for matching and positioning. The richer the image features and the more feature points used for matching, the larger the quality factors τ1 and τ2, and the filter observation noise R calculated thereby k The smaller it is, the more weight the filter uses to use the visual matching results for error estimation and compensation.

[0064] The above-mentioned quality factor calculation method can automatically evaluate the visual matching quality according to information such as the feature richness and the ratio of available feature points in the visual image matching process, including the quality evaluation of the image itself and the matching process, and then adaptively adjust the observation noise matrix according to the image and matching quality, and realize adaptive fusion filtering when the visual matching quality is unstable.

[0065] (4) Implement adaptive filtering estimation based on quality factor.

[0066] The adaptive filtering fusion method based on quality factor is as follows:

[0067] Status prediction: X k,k-1 =Φ k,k-1 X k-1

[0068] State prediction variance:

[0069] Adaptive filter gain: K k =P k,k-1 H k T (H k P k,k-1 H k T +R k ) -1

[0070] Adaptive State Estimation: X k =X k,k-1 +K k (Z k -H k X k,k-1 )

[0071] Adaptive state estimation variance: P k =(IK k H k ) k,k-1 (IK k H k ) T +K k R k K k T

[0072] Among them, X k,k-1 is the one-step prediction value of the filter state, X k-1 is the filter state at time k-1, Φ k,k-1 is the combined navigation state transfer matrix established based on the inertial navigation error law, P k,k-1 is the one-step prediction value of the error covariance, P k-1 is the error covariance at k-1 moment; Q k-1 is the system noise matrix at time k-1, K k is the filter gain matrix at time k, R k is the full profile observation noise matrix at time k, Z k is the observed quantity at time k; H k is the observation matrix at time k; P k is the error covariance at time k, and I is the unit matrix. In the present invention, the subscripts k and k-1 represent the current time and the previous time respectively.

[0073] (5) Compensate and correct the related errors based on the filtering estimation effect.

[0074] Combined with the flight status and filter convergence status, the filter estimation results are used to perform real-time compensation for the system position error, velocity error, attitude error, and device error. The compensation method can refer to the existing method.

[0075] The adaptive filtering method of the present invention is used to test the accuracy of inertial vision integrated navigation. Figure 2 As shown in , the change of visual matching quality factor in one experiment is given, such as Figure 3 As shown, a position error curve of the adaptive filtering of the inertial vision integrated navigation based on the quality factor implemented by the present invention is given, and a position error curve implemented by the existing inertial vision integrated navigation method is also given.

[0076] Depend on Figure 3 It can be seen that the method of the present invention can achieve higher combined navigation accuracy during the flight of the UAV, especially when the visual matching effect fluctuates, the combined navigation accuracy has obvious advantages over the existing methods.

[0077] The present invention proposes an adaptive filtering method for inertial vision integrated navigation based on quality factor, which realizes dynamic adaptation to visual matching results of different qualities and improves the accuracy and reliability of inertial vision integrated navigation in complex scenes.

[0078] Features described and / or illustrated above for one embodiment may be used in the same or similar manner in one or more other embodiments, and / or combined with or used in place of features in other embodiments.

[0079] It should be emphasized that the term "include / comprises" when used herein refers to the presence of features, integers, steps or components, but does not exclude the presence or addition of one or more other features, integers, steps, components or combinations thereof.

[0080] The many features and advantages of these embodiments are apparent from this detailed description, and thus the appended claims are intended to cover all such features and advantages of these embodiments that fall within their true spirit and scope. Furthermore, since numerous modifications and changes will readily occur to those skilled in the art, it is not intended that the embodiments of the invention be limited to the exact construction and operation illustrated and described, but rather all suitable modifications and equivalents falling within the scope thereof are intended to be covered.

[0081] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

[0082] Parts of the present invention that are not described in detail are well known to those skilled in the art.

Claims

1. An adaptive filtering method for inertial vision integrated navigation based on quality factor, characterized in that: The steps include: Construct the adaptive filtering state and observation model of inertial vision integrated navigation; An adaptive calculation method for the observation noise matrix is ​​designed based on the quality factor. The calculation method for the observation noise matrix is: Among them, R k is the filter observation noise matrix, R1 is the set basic image observation noise matrix; R2 is the set matching observation noise matrix; τ1 is the image quality factor, τ2 is the matching quality factor; Perform Kalman adaptive filtering estimation based on the state quantity, observation model, and observation noise matrix; Adaptively compensate and correct the related errors based on the filtering estimation effect; The basic image observation noise matrix and the matching observation noise matrix are respectively expressed as: Among them, σ r1 is the reference image noise; σ r2 The noise is in the real shot. The calculation method of the image quality factor and the matching quality factor is as follows: Among them, A is the number of feature points contained in a preset frame of image, B is the number of feature point pairs with the same name obtained by visual feature matching between the real image and the reference image, and C is the number of inliers obtained by homography estimation based on the feature point pairs with the same name.

2. The inertial vision integrated navigation adaptive filtering method according to claim 1, characterized in that: The state quantity at least includes position error, speed error, misalignment angle, and inertial device error.

3. The inertial vision integrated navigation adaptive filtering method according to claim 2 is characterized in that: The state quantities include the latitude error and longitude error of the inertial navigation, the north and east speed errors of the inertial navigation, the north, celestial and east misalignment angles of the inertial navigation, the accelerometer zero bias in the inertial navigation carrier coordinate system, and the gyro drift in the inertial navigation carrier coordinate system.

4. The inertial vision integrated navigation adaptive filtering method according to claim 3 is characterized in that: The observation model includes observation quantities and an observation matrix; The observed value is the difference between the inertial navigation result and the visual navigation result, expressed as: in, λ ins are the latitude and longitude calculated by inertial navigation respectively; λ vns are the latitude and longitude calculated by visual navigation respectively; The observation matrix is ​​expressed as:

5. The inertial vision integrated navigation adaptive filtering method according to claim 1, characterized in that: σ r1 The value range is 50% to 150% of the reference image resolution. r2 The value range is 50% to 150% of the actual image resolution.

6. The inertial vision integrated navigation adaptive filtering method according to claim 1, characterized in that: The visual feature matching between the real shot image and the reference image adopts the nearest neighbor method or the feature matching network method, and the number of internal points obtained by homography estimation based on the feature point pairs with the same name adopts the random consistency sampling method.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: The processor executes the computer program to implement the method described in any one of claims 1 to 6.