A filtering method for full-profile inertial and visual integrated navigation

Abstract

The present invention provides a full-profile inertial vision integrated navigation filtering method, including establishing a unified full-profile inertial vision integrated navigation filter state equation based on the error state quantity required for navigation; establishing a full-profile inertial vision integrated navigation filter observation equation using visual information characteristics of different flight stages; using the Kalman filtering method to perform full-profile adaptive filtering fusion calculation; and using the filter estimation result to perform full-profile adaptive error compensation. The present invention constructs a unified filter that meets the requirements of integrated navigation throughout the flight process, designs an observation quantity with adaptive selection, and adaptively uses the corresponding available information at each stage of the flight to achieve high-precision integrated navigation.

Description

Technical Field

[0001] The invention belongs to the technical field of inertial vision integrated navigation, and in particular relates to a full-section inertial vision integrated navigation filtering method. Background Art

[0002] Inertial vision combined navigation combines the characteristics of autonomous, continuous, real-time inertial navigation and high precision of visual navigation. It is an effective means for drones and other aircraft to achieve high-precision and strong autonomous navigation and positioning. Existing inertial vision combined navigation generally uses visual navigation results as measurement information, and estimates and corrects inertial navigation errors through corresponding filtering methods. The combined navigation effect is directly related to the filtering method. The visual navigation information and its characteristics that drones can obtain in different flight stages are different. Generally, a specific filtering method designed specifically for a certain flight stage is used. Filter calculations are performed separately in different flight stages to meet specific information fusion requirements, resulting in complex filtering processing at different stages, and difficulty in connecting and switching system states. It cannot meet the high-precision and high-reliability navigation and positioning requirements of the entire profile of drones. Summary of the invention

[0003] The purpose of the present invention is to propose a full-profile inertial vision combined navigation filtering method, which adopts a unified filtering method to cover the information fusion requirements of each stage of UAV flight, adaptively utilizes the corresponding available information at each stage to achieve high-precision combined navigation, and enhances the high-precision autonomous navigation capability of the entire profile of the UAV.

[0004] The technical solution adopted by the present invention to achieve the above-mentioned purpose is as follows:

[0005] As one aspect of the present invention, a full-profile inertial vision integrated navigation filtering method is provided, comprising the following steps:

[0006] According to the error state quantity required for navigation, a unified full-profile inertial and visual integrated navigation filter state equation is established;

[0007] The observation equation of the full-profile inertial-visual integrated navigation filter is established by using the visual information characteristics of different flight stages.

[0008] The state equation and observation equation of the full-profile adaptive inertial vision integrated navigation are respectively expressed as:

[0009] X k,k-1 =Φ k,k-1 X k-1 +W k-1

[0010] Z k =B k H k X k +V k

[0011] Among them, X k,k-1 is the one-step prediction value of the filter state, X k is the filter state quantity; Φ k,k-1 is the combined navigation state transfer matrix established based on the inertial navigation error law; W k-1 is the system random noise vector; Z k is the filter observation; H k is the observation matrix; V k is the observed random noise vector; B k is the full-profile adaptive filtering control matrix; the subscripts k and k-1 represent the current moment and the previous moment respectively;

[0012] The Kalman filter method is used to perform full-profile adaptive filter fusion calculation;

[0013] The filter estimation results are used to perform full profile adaptive error compensation.

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

[0015] Furthermore, the filter observation is the difference between the inertial navigation result and the reference information in different stages of the mission process, expressed as:

[0016]

[0017] in, are the forward and longitudinal velocities of the inertial navigation vehicle system respectively; λ ins are the inertial navigation latitude and longitude respectively; λ0 are the latitude and longitude of the airport origin; is the transformation matrix from the airport coordinate system to the navigation coordinate system, represents the element in the i-th row and j-th column of the matrix, i,j∈[1,2,3]; X vns , Z vns They are the longitudinal displacement and lateral displacement of the UAV relative to the airport origin calculated by visual navigation; λ ins_img are the latitude and longitude of the center point of the visual image calculated by inertial navigation respectively; λ vns_img are the latitude and longitude of the center point of the visual image calculated by visual navigation, and B k is the full profile adaptive filter control matrix.

[0018] Furthermore, the full-profile adaptive filtering control matrix B k It is expressed as:

[0019]

[0020] Among them, b1, b2, b3, b4, b5, and b6 are the set full-profile filtering control coefficients, b1 and b2 are zero-speed control coefficients, b3 and b4 are forward-looking control coefficients, and b5 and b6 are downward-looking control coefficients.

[0021] Furthermore, the method for setting the coefficients of the full-profile filter control matrix is ​​as follows:

[0022] a) If it is in the outbound, standby and inbound stages and is in a static state, then b1=1, b2=1, and the other coefficients are all 0;

[0023] b) If it is in the outbound, standby and inbound stages and is in a uniform linear motion state, then b2 = 1, and the other coefficients are all 0;

[0024] c) If it is in the take-off or landing phase, and the airport runway information identified by visual navigation is valid, then b3=1, b4=1, and the other coefficients are all 0;

[0025] d) If in the climb, cruise or descent phase, and the visual navigation downward image matching is valid, then b5=1, b6=1, and the other coefficients are all 0.

[0026] Furthermore, the observation matrix H k It is expressed as:

[0027]

[0028]

[0029]

[0030] in, is the transformation matrix from the navigation coordinate system to the carrier coordinate system, represents the element in the i-th row and j-th column of the matrix; V n 、V u 、V e They are the north, sky and east speeds of the drone respectively; for The transposed matrix of represents the element in the i-th row and j-th column of the matrix; h is the flight altitude of the drone; θ and γ are the pitch angle and roll angle of the drone respectively; R M , R Nare the meridian radius and the meridian radius of the earth respectively. M and N are intermediate variables. M(i,j) and N(i,j) represent the elements in the i-th row and j-th column of the matrices M and N respectively, i,j∈[1,2,3].

[0031] Furthermore, the full-profile inertial vision integrated navigation filtering method further includes the step of constructing an adaptive full-profile observation noise matrix, wherein the full-profile observation noise matrix R k Expressed as

[0032]

[0033] Among them, σ bv1 , σ bv2 , σ wp1 , σ wp2 , σ np1 , σ np2 is the set full profile observation noise coefficient, σ bv1 , σ bv2 is the zero-speed noise coefficient, σ wp1 , σ wp2 is the forward-looking noise coefficient, σ np1 , σ np2 is the downward noise coefficient.

[0034] Furthermore, the method for calculating the coefficients of the full-profile observation noise matrix includes:

[0035] σ bv1 The value range is 0.05~0.3,σ bv2 The value range is 0.05~0.3;

[0036] σ wp1 , σ wp2 The calculation method is as follows:

[0037]

[0038] Where D is the distance between the UAV and the start point of the runway, σ w0 is the inherent noise in the runway feature recognition result, σ w1 is the distance-related superimposed noise in the runway feature recognition result, A is the starting action distance of the runway feature recognition, and B is the effective action distance of the runway feature recognition;

[0039] σ np1 , σ np2 The calculation method is as follows:

[0040] σ np1 =σ n0 +2h·max(δΦ γ ,δΦ θ ), σ np2=σ np1

[0041] Among them, σ n0 is the inherent noise of the downward image matching, h is the flight height of the UAV, δΦ γ , δΦ θ They are the attitude accuracy of the visual camera on the drone in the roll and pitch directions, max(δΦ γ ,δΦ θ ) means taking δΦ γ , δΦ θ The larger of the two.

[0042] Furthermore, σ w0 The value range is 1 to 10, w1 The value range is 0.5~5, σ n0 The value range is 2 to 20.

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

[0044] (1) The present invention establishes the full-profile inertial and visual combined navigation state equations and observation equations, performs unified information fusion in the entire mission process, and realizes high-precision autonomous navigation and positioning in the entire profile of UAV missions such as unloading, standby, take-off, climbing, cruising, descent, landing, and storage. It can get rid of the dependence on manual navigation guidance in the entire mission process, and significantly reduce the workload of operators.

[0045] (2) The present invention significantly reduces the complexity of combined navigation filtering calculation and navigation state switching during UAV flight through full-profile intelligent filtering and error compensation, improves the reliability of UAV combined navigation, and ensures the safety of UAV autonomous flight.

[0046] (3) The full-profile filtering method of the present invention can adaptively meet the high-precision autonomous navigation requirements of UAVs at different mission stages, significantly improving the adaptability of inertial vision combined navigation, and further improving the autonomous navigation capability of UAVs in performing tasks in complex scenarios.

[0047] (4) The present invention establishes an adaptive full-profile observation noise matrix, which, combined with the constructed filtering equation, has the ability to adaptively select observation quantities and calculate observation noise, and adaptively utilizes the corresponding available information in each stage of flight to achieve high-precision combined navigation, significantly improving the full-profile inertial vision combined navigation performance of UAV flight missions. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0049] Figure 1 A full-section inertial vision integrated navigation filtering flow chart provided for a specific embodiment of the present invention;

[0050] Figure 2 A schematic diagram of the flight trajectory of a drone in an experiment provided by a specific embodiment of the present invention;

[0051] Figure 3 The figure is a schematic diagram comparing the combined navigation result of the present invention and the result of the conventional method in an experiment provided by a specific embodiment of the present invention. DETAILED DESCRIPTION

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

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

[0054] In view of the technical problems existing in the prior art, the present invention proposes a full-profile inertial vision integrated navigation filtering method, comprising: (1) optimizing various error state quantities necessary for realizing high-precision autonomous navigation, and establishing a unified inertial vision integrated navigation filter state equation; (2) utilizing visual information features of different flight stages to establish a full-profile inertial vision integrated navigation filter observation equation; (3) based on a filtering method with the ability to adaptively select observation quantities and calculate an observation matrix, realizing full-profile adaptive filtering fusion calculation; (4) combining the flight state and the filter convergence state, utilizing the filter estimation result to perform full-profile adaptive error compensation on the system position error, velocity error, attitude error, device error, etc.

[0055] The full-profile inertial vision integrated navigation filtering method proposed in the present invention constructs a unified filter that meets the requirements of integrated navigation throughout the flight process, designs a filtering method that can adaptively select observation quantities and calculate the observation matrix, and adaptively uses the corresponding available information in each stage of flight to achieve high-precision integrated navigation, thereby significantly improving the full-profile inertial vision integrated navigation performance of UAV flight missions.

[0056] As a specific embodiment of the present invention, a full-section inertial vision integrated navigation filtering method is provided, such as Figure 1 As shown, the implementation steps include the following:

[0057] (1) Establish the full-profile adaptive inertial vision integrated navigation filtering equation. The filtering equation includes the state equation and the observation equation, which are defined as:

[0058] X k,k-1 =Φ k,k-1 X k-1 +W k-1

[0059] Z k =B k H k X k +V k

[0060] Among them, X k,k-1 is the one-step prediction value of the filter state, X k is the filter state quantity; Φ k,k-1 is the combined navigation state transfer matrix established based on the inertial navigation error law; W k-1 is the system random noise vector; Z k is the filter observation; H k is the observation matrix; V k is the observed random noise vector; B k is the full profile adaptive filter control matrix, which performs adaptive control on the filter at different flight stages. In the present invention, the subscripts k and k-1 represent the current moment and the previous moment respectively.

[0061] (2) Optimize the various error states required for high-precision autonomous navigation and establish a unified inertial and visual combined navigation filter state for the entire profile.

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

[0063]

[0064] in, δλ are the latitude error and longitude error of inertial navigation respectively; δVn ,δ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.

[0065] (3) Utilize the visual information characteristics of different flight phases to establish the full-profile inertial vision integrated navigation filter observation.

[0066] Observation quantity Z k It is the difference between the inertial navigation result and the reference information at different stages of the mission process, defined as:

[0067]

[0068] in, are the forward and longitudinal velocities of the inertial navigation vehicle system respectively; λ ins are the inertial navigation latitude and longitude respectively; λ0 are the latitude and longitude of the airport origin; is the transformation matrix from the airport coordinate system to the navigation coordinate system, represents the element in the i-th row and j-th column of the matrix, i,j∈[1,2,3]; X vns , Z vns They are the longitudinal displacement and lateral displacement of the UAV relative to the airport origin calculated by visual navigation; λ ins_img are the latitude and longitude of the center point of the visual image calculated by inertial navigation respectively; λ vns_img are the latitude and longitude of the center point of the visual image calculated by visual navigation, and B k It is the full-profile adaptive filter control matrix, which performs adaptive control on the filter in different flight phases.

[0069] The above observations cover the entire process of the drone's outbound, standby, take-off, climb, cruise, descent, landing, and storage. In the outbound, standby, and storage stages, the drone is stationary or moving at a uniform speed. The error is estimated by using the characteristic of zero; during the take-off and landing phases, the relative displacement of the UAV relative to the airport origin is obtained through visual navigation and converted into the latitude and longitude of the UAV, and the difference between the latitude and longitude of inertial navigation and the latitude and longitude of the UAV is used as the observation quantity for error estimation; during the climb, cruise and descent phases, the latitude and longitude of the center point of the image are obtained through visual navigation and inertial navigation respectively, and the difference between the two is used as the observation quantity for error estimation. Through the construction of the above observation quantities, combined navigation error correction can be performed on the entire UAV flight profile to meet the high-precision navigation and positioning requirements at each stage.

[0070] B k is the full profile adaptive filter control matrix, defined as:

[0071]

[0072] Among them, b1, b2, b3, b4, b5, and b6 are set full-profile filter control coefficients, b1 and b2 are zero-speed control coefficients, b3 and b4 are forward-looking control coefficients, and b5 and b6 are downward-looking control coefficients, which are automatically set by the integrated navigation program at different stages of the flight process according to the type and quality of the visual navigation information obtained. In this embodiment, the full-profile filter control coefficient setting method is as follows:

[0073] a) If it is in the outbound, standby and inbound stages and is in a static state, then b1=1, b2=1, and the other coefficients are all 0;

[0074] b) If it is in the outbound, standby and inbound stages and is in a uniform linear motion state, then b2 = 1, and the other coefficients are all 0;

[0075] c) If it is in the take-off or landing phase, and the airport runway information identified by visual navigation is valid, then b3=1, b4=1, and the other coefficients are all 0;

[0076] d) If in the climb, cruise and descent phases, and the visual navigation downward image matching is valid, then b5 = 1, b6 = 1, and the other coefficients are all 0;

[0077] (4) Derive the relationship between the visual navigation results and inertial navigation results and their errors at different stages, and establish the full-profile inertial and visual combined navigation filter observation matrix.

[0078] The observation matrix H k Defined as:

[0079]

[0080]

[0081]

[0082] in, is the transformation matrix from the navigation coordinate system to the carrier coordinate system, which is a 3×3 matrix. V represents the element in the i-th row and j-th column of the matrix, i,j∈[1,2,3]; n 、V u 、V e They are the north, sky and east speeds of the drone respectively; for The transposed matrix of represents the element in the i-th row and j-th column of the matrix; h is the flight altitude of the drone; θ and γ are the pitch angle and roll angle of the drone respectively; R M , R N are the meridian radius and the meridian radius of the earth respectively. M and N are intermediate variables. M(i,j) and N(i,j) represent the elements in the i-th row and j-th column of the matrices M and N respectively, i,j∈[1,2,3].

[0083] (5) Full-profile adaptive filtering fusion is performed based on the above state equations and observation equations.

[0084] The full profile adaptive filtering fusion method is as follows:

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

[0086] State prediction variance:

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

[0088] Adaptive state estimation: X k =X k,k-1 +K k (Z k -(B k H k )X k,k-1 )

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

[0090] 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, B k is the full profile adaptive filter control 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.

[0091] In Kalman filter fusion, the full profile observation noise matrix R k Usually determined based on experience.

[0092] (6) Full profile adaptive error compensation using filter estimation results

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

[0094] As another specific embodiment of the present invention, in order to optimize the Kalman filter fusion calculation accuracy, an adaptive full-profile observation noise matrix R is designed. k , expressed as:

[0095]

[0096] Among them, σ bv1 , σ bv2 , σ wp1 , σ wp2, σ np1 , σ np2 is the set full profile observation noise coefficient, σ bv1 , σ bv2 is the zero-speed noise coefficient, σ wp1 , σ wp2 is the forward-looking noise coefficient, σ np1 , σ np2 is the downward noise coefficient, which is set according to the flight status, the type and quality of visual navigation information during the flight, and the filtering effect is adaptively controlled in different flight stages. In this embodiment, the method for setting the full profile observation noise coefficient is as follows:

[0097] a) σ bv1 , σ bv2 Characterizes the speed noise of the UAV when it is stationary or in uniform linear motion. It is generally set to a smaller fixed value. Preferably, σ bv1 The value range is 0.05~0.3,σ bv2 The value range is 0.05 to 0.3. In this embodiment, σ is set bv1 =0.1,σ bv2 =0.1.

[0098] b)σ wp1 , σ wp2 The noise that characterizes the runway feature recognition results of the drone during takeoff and landing is related to the distance between the drone and the runway. The setting method is as follows:

[0099]

[0100] Where D is the distance between the drone and the start point of the runway, which changes with time during takeoff and landing, σ w0 is the inherent noise in the runway feature recognition result, σ w1 is the distance-related superimposed noise in the runway feature recognition result, A is the starting distance of runway feature recognition, and B is the effective distance of runway feature recognition. w0 , σ w1 It can be set according to the runway feature recognition method and application scenario requirements. Preferably, σ w0 , σ w1 The value ranges are 1 to 10 and 0.5 to 5. In this embodiment, σ w0 =2,σ w1 =5, A=6000, B=1500. In different applications, the above setting values ​​can be adjusted according to the application scenario.

[0101] The above method fully considers the influence of the distance between the UAV and the runway on the runway feature recognition result, and can adaptively control σ according to the distance from the runway. wp1 , σ wp2When the distance to the airport runway is far, the accuracy of runway feature recognition is poor, and the calculated σ wp1 , σ wp2 The control filter uses the recognition results to estimate and correct the error to a certain extent. As the UAV gets closer to the airport runway, the accuracy of runway feature recognition becomes higher and higher, and the calculated σ wp1 , σ wp2 The value decreases accordingly, and the control filter uses a larger weight to fully estimate and correct the error using the high-precision recognition result to obtain a more accurate combined navigation result.

[0102] c)σ np1 , σ np2 Characterizes the noise of downward image matching in the climb, cruise and descent phases, which is related to the flight altitude, attitude accuracy, etc. The setting method is as follows:

[0103] σ np1 =σ n0 +2h·max(δΦ γ ,δΦ θ ), σ np2 =σ np1

[0104] Among them, σ n0 is the inherent noise of the downward image matching, h is the flight height of the UAV, δΦ γ , δΦ θ They are the attitude accuracy of the visual camera on the drone in the roll and pitch directions, max(δΦ γ ,δΦ θ ) means taking δΦ γ , δΦ θ The larger of the two. n0 Set according to the application scenario. Preferably, σ n0 The value range is 2 to 20. In this embodiment, σ n0 =8,δΦ γ =0.1 / 57.3,δΦ θ =0.1 / 57.3. In different applications, the above setting values ​​can be adjusted according to the application scenarios.

[0105] The above method fully considers the impact of flight altitude and attitude accuracy on the downward image matching results, and can adaptively control σ according to the flight altitude and attitude accuracy of the UAV. np1 , σ np2 When the flight altitude is high and the attitude accuracy is poor, the calculated σ np1 , σ np2 The control filter uses the matching result to correct the error to a certain extent. When the flight altitude is low and the attitude accuracy is high, the matching accuracy is high. At this time, the calculated σnp1 , σ np2 The control filter uses a larger weight to fully estimate and correct the error using the high-precision matching result to obtain a more accurate combined navigation result.

[0106] The full-profile inertial vision integrated navigation filtering method of the present invention is used to test the accuracy of the full-profile inertial vision integrated navigation. Figure 2 As shown in the figure, the full profile flight trajectory of the UAV in an experiment is given, such as Figure 3 As shown, a position error curve of the full-profile inertial vision integrated navigation implemented based on the method of the present invention is given, and a position error curve implemented by a conventional staged inertial vision integrated navigation method is also given.

[0107] Depend on Figure 3 It can be seen that the method of the present invention can realize high-precision inertial and visual combined navigation throughout the entire UAV flight process, especially when the flight state changes (such as the aircraft climbing or descending), the advantage is more obvious than the conventional phased method.

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

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

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

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

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

Claims

1. A full-section inertial vision integrated navigation filtering method, characterized in that: The steps include: According to the error state quantity required for navigation, a unified full-profile inertial and visual integrated navigation filter state equation is established; The observation equation of the full-profile inertial-visual integrated navigation filter is established by using the visual information characteristics of different flight stages. The state equation and observation equation of the full-profile inertial vision integrated navigation filter are respectively expressed as: X k,k-1 =Φ k,k-1 X k-1 +W k-1 Z k =B k H k X k +V k Among them, X k,k-1 is the one-step prediction value of the filter state, X k is the filter state quantity; Φ k,k-1 is the combined navigation state transfer matrix established based on the inertial navigation error law; W k-1 is the system random noise vector; Z k is the filter observation; H k is the observation matrix; V k is the observed random noise vector; B k is the full-profile adaptive filtering control matrix; the subscripts k and k-1 represent the current moment and the previous moment respectively; The Kalman filter method is used to perform full-profile adaptive filter fusion calculation; The filter estimation results are used to perform full profile adaptive error compensation; The filter observation is the difference between the inertial navigation result and the reference information in different stages of the mission process, expressed as: in, are the forward and longitudinal velocities of the inertial navigation vehicle system respectively; λ ins are the inertial navigation latitude and longitude respectively; λ0 are the latitude and longitude of the airport origin; is the transformation matrix from the airport coordinate system to the navigation coordinate system, represents the element in the i-th row and j-th column of the matrix, i,j∈[1,2,3]; X vns , Z vns They are the longitudinal displacement and lateral displacement of the UAV relative to the airport origin calculated by visual navigation; λ ins_img are the latitude and longitude of the center point of the visual image calculated by inertial navigation respectively; λ vns_img are the latitude and longitude of the center point of the visual image calculated by visual navigation, and B k is the full profile adaptive filter control matrix; The full profile adaptive filtering control matrix B k It is expressed as: Among them, b1, b2, b3, b4, b5, and b6 are the set full-profile filter control coefficients, b1 and b2 are zero-speed control coefficients, b3 and b4 are forward-looking control coefficients, and b5 and b6 are downward-looking control coefficients; The method for setting the coefficients of the full-profile filter control matrix is ​​as follows: If it is in the outbound, standby and inbound stages and is in a static state, then b1=1, b2=1, and other coefficients are all 0; If it is in the outbound, standby and inbound stages, and is in a uniform linear motion state, then b2 = 1, and the other coefficients are all 0; If it is in the take-off or landing phase, and the airport runway information identified by visual navigation is valid, then b3=1, b4=1, and the other coefficients are all 0; If it is in the climb, cruise or descent phase, and the visual navigation downward image matching is valid, then b5=1, b6=1, and the other coefficients are all 0; The observation matrix H k It is expressed as: in, is the transformation matrix from the navigation coordinate system to the carrier coordinate system, represents the element in the i-th row and j-th column of the matrix; V n 、V u 、V e They are the north, sky and east speeds of the drone respectively; for The transposed matrix of represents the element in the i-th row and j-th column of the matrix; h is the flight altitude of the drone; θ and γ are the pitch angle and roll angle of the drone respectively; R M , R N are the meridian radius and the meridian radius of the earth respectively. M and N are intermediate variables. M(i,j) and N(i,j) represent the elements in the i-th row and j-th column of the matrices M and N respectively, i,j∈[1,2,3].

2. The full-section inertial vision integrated navigation filtering method according to claim 1, characterized in that: The filter state quantity at least includes: latitude error and longitude error of inertial navigation, north and east velocity errors of inertial navigation, north, celestial and east misalignment angles of inertial navigation, accelerometer zero bias in three orthogonal directions on the inertial navigation carrier coordinate system, and gyro drift in three orthogonal directions on the inertial navigation carrier coordinate system.

3. The full-section inertial vision integrated navigation filtering method according to claim 1, characterized in that: The full-profile inertial vision integrated navigation filtering method also includes the step of constructing an adaptive full-profile observation noise matrix, wherein the full-profile observation noise matrix R k Expressed as Among them, σ bv1 , σ bv2 , σ wp1 , σ wp2 , σ np1 , σ np2 is the set full profile observation noise coefficient, σ bv1 , σ bv2 is the zero-speed noise coefficient, σ wp1 , σ wp2 is the forward-looking noise coefficient, σ np1 , σ np2 is the downward noise coefficient.

4. The full-section inertial vision integrated navigation filtering method according to claim 3 is characterized in that: The method for calculating the coefficients of the full-profile observation noise matrix includes: σ bv1 The value range is 0.05~0.3,σ bv2 The value range is 0.05~0.3; σ wp1 , σ wp2 The calculation method is as follows: Where D is the distance between the UAV and the start point of the runway, σ w0 is the inherent noise in the runway feature recognition result, σ w1 is the distance-related superimposed noise in the runway feature recognition result, A is the starting action distance of the runway feature recognition, and B is the effective action distance of the runway feature recognition; σ np1 , σ np2 The calculation method is as follows: s np1 =s n0 +2h max(δΦ γ ,δΦ θ ),s np2 =s np1 Among them, σ n0 is the inherent noise of the downward image matching, h is the flight height of the UAV, δΦ γ , δΦ θ They are the attitude accuracy of the visual camera on the drone in the roll and pitch directions, max(δΦ γ ,δΦ θ ) means taking δΦ γ , δΦ θ The larger of the two.

5. The full-section inertial vision integrated navigation filtering method according to claim 4 is characterized in that: σ w0 The value range is 1 to 10, w1 The value range is 0.5~5, σ n0 The value range is 2 to 20.

6. 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 5.

Images