Inertial vision integrated navigation error tracking and compensating method

Abstract

The invention provides an inertia and vision integrated navigation error tracking and compensating method. The method comprises the following steps: constructing an unmanned aerial vehicle inertia / vision integrated navigation filtering state variable; taking position information obtained by visual navigation as observed quantity, and constructing an inertia / visual integrated navigation filtering observation equation of the unmanned aerial vehicle; establishing a combined navigation error estimation evaluation model based on a historical estimation value, and calculating an error estimation evaluation factor lambda k; performing inertial / visual integrated navigation filtering estimation based on the integrated navigation filtering state variable, the observation equation and the error estimation evaluation factor; and according to the obtained inertia / vision integrated navigation filtering estimation, carrying out real-time compensation on an integrated navigation error. According to the method, the visual navigation result can be effectively tracked under the conditions of state abrupt change, carrier maneuvering and the like, the utilization effect of visual navigation observed quantity is greatly improved, and the integrated navigation error estimation precision and efficiency are improved.

Description

Technical Field

[0001] This invention belongs to the field of inertial vision integrated navigation technology, and specifically relates to an inertial vision integrated navigation error tracking and compensation method. Background Technology

[0002] Inertial-visual integrated navigation combines the advantages of inertial navigation (comprehensive parameters, no geographical limitations, high short-term relative accuracy) with visual navigation (high absolute positioning accuracy, and error non-divergence over time), making it an effective means for UAVs and other aircraft to achieve high-precision, highly autonomous navigation and positioning. However, in engineering applications, inertial-visual integrated navigation is affected by many factors such as long-duration UAV flights, complex profile maneuvers, and diverse terrain. Filters struggle to achieve stable estimation and accurate compensation of various errors under complex and changing conditions, resulting in the integrated navigation performance being significantly affected by the application scenario and unable to consistently provide high-precision, high-consistency navigation and positioning results. Summary of the Invention

[0003] The purpose of this invention is to overcome the shortcomings of the prior art and provide an error tracking and compensation method for inertial vision integrated navigation. The solution of this invention can solve the problems existing in the prior art.

[0004] The technical solution of this invention:

[0005] According to the first aspect, an inertial vision integrated navigation error tracking and compensation method is provided, including the following steps:

[0006] Step 1: Construct the UAV inertial / visual integrated navigation filter state variables;

[0007] Step 2: Using the position information obtained from visual navigation as the observation, construct the UAV inertial / visual combined navigation filtering observation equation;

[0008] Step 3: Establish an integrated navigation error estimation and evaluation model based on historical estimates, and calculate the error estimation evaluation factor λ. k ;

[0009] The integrated navigation error estimation and evaluation model is as follows:

[0010]

[0011] in,

[0012]

[0013] Where, λ k η is the evaluation factor for error estimation. k N is the evaluation coefficient; k M is the residual matrix; k R is the prediction matrix; kH represents the observation noise matrix at time k; k Let Q be the observation matrix at time k; k-1 Φ is the system noise matrix at time k-1; k,k-1 P is the state transition matrix for integrated navigation; k-1 e is the error covariance at time k-1; k To output the residual; These are the residual variance matrices at time k-1 and time k, respectively. ρ is the one-step prediction value of the filter state variables; β is the forgetting factor; and the values ​​of both are determined based on the flight status and visual navigation quality.

[0014] Step 4: Based on the integrated navigation filter state variables, observation equations, and error estimation evaluation factors, perform inertial / visual integrated navigation filter estimation;

[0015] Step 5: Based on the obtained inertial / visual integrated navigation filter estimation, perform real-time compensation for integrated navigation errors.

[0016] Furthermore, the filter state variable is:

[0017]

[0018] in, δλ represents the latitude error and longitude error of inertial navigation, respectively; δV n ,δV e These represent the northbound and eastbound velocity errors of the inertial navigation system, respectively; φ n φ u φ e These are the north, sky, and east offset angles for inertial navigation, respectively. These represent the accelerometer zero bias in the coordinate system of the inertial navigation vehicle; ε x ε y ε z These represent the gyroscope drift in the coordinate system of the inertial navigation vehicle.

[0019] Furthermore, the observation Z k for:

[0020]

[0021] in, λ ins These are the latitude and longitude calculated by inertial navigation, respectively. λ vns These are the latitude and longitude calculated by visual navigation, respectively.

[0022] Observation matrix H k Defined as:

[0023]

[0024] Furthermore, the method for calculating the integrated navigation filter is as follows:

[0025] State prediction: X k,k-1 =Φ k,k-1 X k-1

[0026] State prediction variance:

[0027] Filter gain: K k =P k,k-1 H k T (H k P k,k-1 H k T +R k ) -1

[0028] State estimation: X k =X k,k-1 +K k (Z k -H k X k,k-1 )

[0029] Variance of state estimation: P k =(IK k H k )P k,k-1 (IK k H k ) T +K k R k K k T

[0030] Among them, X k,k-1 X is the one-step prediction value of the filter state quantity. k-1 Let Φ be the filter state variable at time k-1. k,k-1 P is the integrated navigation state transition matrix established based on the inertial navigation error law. k,k-1 P is the one-step predicted value of the error covariance. k-1 Let Q be the error covariance at time k-1; k-1 Let K be the system noise matrix at time k-1. k Let R be the filter gain matrix at time k. k Let Z be the noise matrix of the full profile observation at time k. k H represents the observation at time k; k P is the observation matrix at time k; kLet I be the error covariance at time k, and I be the identity matrix. In this invention, the subscripts k and k-1 represent the current time and the previous time, respectively.

[0031] According to a second aspect, an electronic device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the inertial vision integrated navigation error tracking and compensation method as described above.

[0032] According to a third aspect, a non-transitory computer-readable storage medium is provided, on which a computer program is stored, which, when executed by a processor, implements the steps of the inertial vision integrated navigation error tracking and compensation method as described above.

[0033] The beneficial effects of this invention compared to the prior art are as follows:

[0034] This invention proposes an error tracking and compensation method for inertial vision-integrated navigation. Based on historical estimates, the method comprehensively evaluates the error estimation effect of integrated navigation and can effectively track visual navigation results under conditions such as sudden changes in state and vehicle maneuvering. This significantly improves the utilization of visual navigation observations and enhances the accuracy and efficiency of integrated navigation error estimation. Attached Figure Description

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

[0036] Figure 1 A flowchart of an inertial vision integrated navigation error tracking and compensation method provided for a specific embodiment of the present invention;

[0037] Figure 2 The results of the integrated navigation of the present invention are compared with those of conventional methods in a test provided for a specific embodiment of the present invention. Detailed Implementation

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

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

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

[0041] like Figure 1 As shown, according to an embodiment of the present invention, an inertial vision integrated navigation error tracking and compensation method is provided, comprising the following steps:

[0042] Step 1: Construct the UAV inertial / visual integrated navigation filter state variables;

[0043] Step 2: Using the position information obtained from visual navigation as the observation, construct the UAV inertial / visual combined navigation filtering observation equation;

[0044] Step 3: Establish an integrated navigation error estimation and evaluation model based on historical estimates, and calculate the error estimation evaluation factor λ. k ;

[0045] The integrated navigation error estimation and evaluation model is as follows:

[0046]

[0047] in,

[0048]

[0049] Where, λ k η is the evaluation factor for error estimation. k N is the evaluation coefficient; k M is the residual matrix; k R is the prediction matrix; k H represents the observation noise matrix at time k; k Let Q be the observation matrix at time k; k-1 Φ is the system noise matrix at time k-1; k,k-1 P is the state transition matrix for integrated navigation; k-1 e is the error covariance at time k-1; k To output the residual; These are the residual variance matrices at time k-1 and time k, respectively. ρ is the one-step prediction value of the filter state variables; β is the forgetting factor; and the values ​​of both are determined based on the flight status and visual navigation quality.

[0050] Step 4: Based on the integrated navigation filter state variables, observation equations, and error estimation evaluation factors, perform inertial / visual integrated navigation filter estimation;

[0051] Step 5: Based on the obtained inertial / visual integrated navigation filter estimation, perform real-time compensation for integrated navigation errors.

[0052] By taking the above steps, the effect of integrated navigation error estimation is comprehensively evaluated based on historical estimates. This enables effective tracking of visual navigation results under conditions such as sudden changes in state and vehicle maneuvering, significantly improving the utilization of visual navigation observations and enhancing the accuracy and efficiency of integrated navigation error estimation.

[0053] In another embodiment, the filter state variable is:

[0054]

[0055] in, δλ represents the latitude error and longitude error of inertial navigation, respectively; δV n ,δV e These represent the northbound and eastbound velocity errors of the inertial navigation system, respectively; φ n φ u φ eThese are the north, sky, and east offset angles for inertial navigation, respectively. These represent the accelerometer zero bias in the coordinate system of the inertial navigation vehicle; ε x ε y ε z These represent the gyroscope drift in the coordinate system of the inertial navigation vehicle;

[0056] In one further embodiment, the observed Z k for:

[0057]

[0058] in, λ ins These are the latitude and longitude calculated by inertial navigation, respectively. λ vns These are the latitude and longitude calculated by visual navigation, respectively.

[0059] Observation matrix H k Defined as:

[0060]

[0061] In one further embodiment, the combined navigation filter calculation method is as follows:

[0062] State prediction: X k,k-1 =Φ k,k-1 X k-1

[0063] State prediction variance:

[0064] Filter gain: K k =P k,k-1 H k T (H k P k,k-1 H k T +R k ) -1

[0065] State estimation: X k =X k,k-1 +K k (Z k -H k X k,k-1 )

[0066] Variance of state estimation: P k =(IK k H k )P k,k-1 (IK k Hk ) T +K k R k K k T

[0067] Among them, X k,k-1 X is the one-step prediction value of the filter state quantity. k-1 Let Φ be the filter state variable at time k-1. k,k-1 P is the integrated navigation state transition matrix established based on the inertial navigation error law. k,k-1 P is the one-step predicted value of the error covariance. k-1 Let Q be the error covariance at time k-1; k-1 Let K be the system noise matrix at time k-1. k Let R be the filter gain matrix at time k. k Let Z be the noise matrix of the full profile observation at time k. k H represents the observation at time k; k P is the observation matrix at time k; k Let I be the error covariance at time k, and I be the identity matrix. In this invention, the subscripts k and k-1 represent the current time and the previous time, respectively.

[0068] According to a second aspect embodiment, an electronic device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the inertial vision integrated navigation error tracking and compensation method as described above.

[0069] According to a third aspect embodiment, a non-transitory computer-readable storage medium is provided, on which a computer program is stored, which, when executed by a processor, implements the steps of the inertial vision integrated navigation error tracking and compensation method as described above.

[0070] To gain a better understanding of the inertial vision integrated navigation error tracking and compensation method provided by the present invention, a detailed description is provided below with reference to specific examples and accompanying drawings.

[0071] The specific implementation methods of this invention are as follows:

[0072] (1) Construct the state equation of UAV inertial / visual integrated navigation filter to cover the main error terms of integrated navigation.

[0073] Filter state variable X k The 13-dimensional state variables, including position error, velocity error, misalignment angle, and inertial device error, are defined as follows:

[0074]

[0075] in, δλ represents the latitude error and longitude error of inertial navigation, respectively; δV n ,δV e These represent the northbound and eastbound velocity errors of the inertial navigation system, respectively; φ n φ u φ e These are the north, sky, and east offset angles for inertial navigation, respectively. These represent the accelerometer zero bias in the coordinate system of the inertial navigation vehicle; ε x ε y ε z These represent the gyroscope drift in the coordinate system of the inertial navigation vehicle.

[0076] (2) Using the position information obtained by visual navigation as the observation, construct the UAV inertial / visual combined navigation filtering observation equation.

[0077] Observation Z k The difference between the inertial navigation result and the visual navigation result is defined as...

[0078]

[0079] in, λ ins These are the latitude and longitude calculated by inertial navigation, respectively. λ vns These are the latitude and longitude calculated by visual navigation, respectively.

[0080] Observation matrix H k Defined as:

[0081]

[0082] (3) Design an evaluation model for combined navigation error estimation based on historical estimates and calculate the error estimation evaluation factor.

[0083] The calculation method for the error estimation evaluation factor is as follows:

[0084]

[0085] in,

[0086]

[0087] ρ is the forgetting factor, and β is the weakening factor. The values ​​of both are determined based on the flight status and visual navigation quality.

[0088] (4) Based on the state equation, observation equation and error estimation evaluation factor of the integrated navigation filter, perform inertial / visual integrated navigation filter calculation.

[0089] The calculation method for integrated navigation filtering is as follows:

[0090] State prediction: X k,k-1 =Φ k,k-1 X k-1

[0091] State prediction variance:

[0092] Filter gain: K k =P k,k-1 H k T (H k P k,k-1 H k T +R k ) -1

[0093] State estimation: X k =X k,k-1 +K k (Z k -H k X k,k-1 )

[0094] Variance of state estimation: P k =(IK k H k )P k,k-1 (IK k H k ) T +K k R k K k T

[0095] Among them, X k,k-1 X is the one-step prediction value of the filter state quantity. k-1 Let Φ be the filter state variable at time k-1. k,k-1 P is the integrated navigation state transition matrix established based on the inertial navigation error law. k,k-1 P is the one-step predicted value of the error covariance. k-1 Let Q be the error covariance at time k-1; k-1 Let K be the system noise matrix at time k-1. k Let R be the filter gain matrix at time k. k Let Z be the noise matrix of the full profile observation at time k. k H represents the observation at time k; k P is the observation matrix at time k; k Let I be the error covariance at time k, and I be the identity matrix. In this invention, the subscripts k and k-1 represent the current time and the previous time, respectively.

[0096] (5) Based on the filter estimation effect, adaptive correction is performed on the position error and velocity error of the integrated navigation.

[0097] By combining the flight status and the filter convergence status, the system position error, velocity error, etc. are compensated in real time using the filter estimation results. The compensation method can refer to existing methods.

[0098] Implementation results:

[0099] The method of this invention is used to test the effect of inertial / visual combined navigation in typical scenarios. Figure 2 The inertial / visual integrated navigation position and visual navigation position curves obtained based on the method of this invention are presented, and the position curves obtained by conventional methods are also presented.

[0100] Depend on Figure 2 As can be seen, the method of the present invention can effectively track the visual navigation results. While tracking the actual flight trajectory, it suppresses the jumps in the visual navigation results, maximizes the role of visual navigation and avoids the impact of its jumps on the integrated navigation results, and significantly improves the accuracy and reliability of integrated navigation.

[0101] The above are merely preferred embodiments of the present invention and are not intended to limit the present 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 method for error tracking and compensation in inertial vision integrated navigation, characterized in that, Includes the following steps: Step 1: Construct the UAV inertial / visual integrated navigation filter state variables; Step 2: Using the position information obtained from visual navigation as the observation, construct the UAV inertial / visual combined navigation filtering observation equation; Step 3: Establish an integrated navigation error estimation and evaluation model based on historical estimates, and calculate the error estimation evaluation factor λ. k ; The integrated navigation error estimation and evaluation model is as follows: in, Where, λ k η is the evaluation factor for error estimation. k N is the evaluation coefficient; k M is the residual matrix; k R is the prediction matrix; k H represents the observation noise matrix at time k; k Let Q be the observation matrix at time k; k-1 Φ is the system noise matrix at time k-1; k,k-1 P is the state transition matrix for integrated navigation; k-1 Let k-1 be the error covariance. e k To output the residual; These are the residual variance matrices at time k-1 and time k, respectively. ρ is the one-step prediction value of the filter state variables; β is the forgetting factor; and the values ​​of both are determined based on the flight status and visual navigation quality. Step 4: Based on the integrated navigation filter state variables, observation equations, and error estimation evaluation factors, perform inertial / visual integrated navigation filter estimation; Step 5: Based on the obtained inertial / visual integrated navigation filter estimation, perform real-time compensation for integrated navigation errors.

2. The inertial vision integrated navigation error tracking and compensation method according to claim 1, characterized in that, The filter state variable is: in, δλ represents the latitude error and longitude error of inertial navigation, respectively; δV n ,δV e These represent the northbound and eastbound velocity errors of the inertial navigation system, respectively; φ n φ u φ e These are the north, sky, and east offset angles for inertial navigation, respectively. These represent the accelerometer zero bias in the coordinate system of the inertial navigation vehicle; ε x ε y ε z These represent the gyroscope drift in the coordinate system of the inertial navigation vehicle.

3. The inertial vision integrated navigation error tracking and compensation method according to claim 2, characterized in that, Observation Z k for: in, λ ins These are the latitude and longitude calculated by inertial navigation, respectively. λ vns These are the latitude and longitude calculated by visual navigation, respectively. Observation matrix H k Defined as:

4. The inertial vision integrated navigation error tracking and compensation method according to claim 3, characterized in that, The combined navigation filter calculation method is as follows: State prediction: X k,k-1 =Φ k,k-1 X k-1 State prediction variance: Filter gain: K k =P k,k-1 H k T (H k P k,k-1 H k T +R k ) -1 State estimation: X k =X k,k-1 +K k (Z k -H k X k,k-1 ) Variance of state estimation: P k =(IK k H k )P k,k-1 (IK k H k ) T +K k R k K k T Among them, X k,k-1 X is the one-step prediction value of the filter state quantity. k-1 Let Φ be the filter state variable at time k-1. k,k-1 P is the integrated navigation state transition matrix established based on the inertial navigation error law. k,k-1 P is the one-step predicted value of the error covariance. k-1 Let Q be the error covariance at time k-1; k-1 Let K be the system noise matrix at time k-1. k Let R be the filter gain matrix at time k. k Let Z be the noise matrix of the full profile observation at time k. k H represents the observation at time k; k P is the observation matrix at time k; k Let I be the error covariance at time k, and I be the identity matrix. In this invention, the subscripts k and k-1 represent the current time and the previous time, respectively.

5. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of the inertial vision integrated navigation error tracking and compensation method as described in any one of claims 1 to 4.

6. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the inertial vision integrated navigation error tracking and compensation method as described in any one of claims 1 to 4.

Images