CN-122015815-A - Inertial/satellite/vision self-adaptive integrated navigation method and system

CN122015815ACN 122015815 ACN122015815 ACN 122015815ACN-122015815-A

Abstract

The invention provides an inertial/satellite/vision self-adaptive combined navigation method and a system, wherein the method comprises the steps of carrying out inertial measurement and navigation calculation to obtain inertial data; the method comprises the steps of calculating satellite navigation observance according to satellite navigation data and inertial data, constructing a satellite observation model, calculating visual navigation observance according to visual navigation data and inertial data, constructing a visual observation model, respectively adopting an adaptive covariance estimation method based on innovation aiming at the satellite and the visual observation model to obtain a satellite and visual observation noise covariance update value, constructing satellite and visual navigation health degrees, calculating satellite and visual fusion weights according to the satellite and the visual navigation health degrees, controlling satellite and visual navigation observance update according to the satellite and the visual fusion weights, calculating weighted equivalent observation matrix and noise covariance, and carrying out filter estimation. According to the invention, a self-adaptive noise estimation and sensor health degree estimation mechanism is introduced, so that dynamic weighted fusion of multi-sensor data is realized, and the robustness and accuracy of the navigation system in complex environments such as satellite signal lock loss and visual characteristic loss are improved.

Inventors

WANG PENGYU
LI WEI
ZONG WENPENG
CHEN ANSHENG
LIN MENGNA
HOU FENGXIA
YUAN SHUBO
LIU LEI
YU JIEPING

Assignees

北京自动化控制设备研究所

Dates

Publication Date: 20260512
Application Date: 20251218

Claims (10)

1. An inertial/satellite/visual self-adaptive combined navigation method is characterized by comprising the following steps: inertial measurement and navigation solution are carried out to obtain inertial data; calculating satellite navigation observance according to the satellite navigation data and the inertial data, and constructing a satellite observation model; calculating visual navigation observance according to the visual navigation data and the inertial data, and constructing a visual observation model; aiming at a satellite observation model and a visual observation model, respectively adopting an adaptive covariance estimation method based on innovation to obtain a satellite observation noise covariance update value and a visual observation noise covariance update value; Constructing satellite navigation health degree and visual navigation health degree, and calculating satellite fusion weight and visual fusion weight according to the satellite navigation health degree and the visual navigation health degree; If the satellite fusion weight is greater than the first weight threshold, updating the satellite navigation observed quantity, and entering the next step, otherwise, directly entering the next step; and calculating the weighted equivalent observation matrix and noise covariance, and carrying out filtering estimation.
2. The method of claim 1, wherein the satellite observation model is: z gnss ＝h gnss x+r gnss Wherein z gnss is satellite navigation observed quantity, at least comprises a difference value between a satellite position and an inertial position and a difference value between a satellite speed and an inertial speed, h gnss is a satellite observation matrix, r gnss is a satellite measurement noise sequence, and x is a state vector; The visual observation model is as follows: z vis ＝h vis x+r vis wherein z vis is visual navigation observed quantity, at least comprises a difference value between a visual position and an inertial position and a difference value between a visual gesture and an inertial gesture, h vis is a visual observation matrix, and r vis is a visual measurement noise sequence.
3. The method of claim 2, wherein h gnss ＝[I 6 0 6×9 ],r gnss is white noise with a mean of 0 and a variance of R gnss ; R vis is white noise with a mean of 0 and a variance of R vis .
4. The method of claim 1, wherein the employing an innovation-based adaptive covariance estimation method comprises the steps of: Calculating the innovation; Calculating an innovation covariance estimated value in a set sliding window; calculating theoretical innovation covariance; Constructing a noise covariance adjustment factor, wherein the noise covariance adjustment factor is the ratio of the trace of the innovation covariance estimation value to the trace of the theoretical innovation covariance; and multiplying the observed noise covariance by a noise covariance adjustment factor to obtain an observed noise covariance update value.
5. The method of claim 4, wherein the innovation calculation method is as follows: Wherein, gamma k is the innovation of the kth filtering period, z k is the actual observed quantity of the kth filtering period, h k is the observed matrix of the kth filtering period, A predictive state vector for the kth filtering period; The method for calculating the innovation covariance estimated value comprises the following steps: wherein P γ,k is the innovation covariance estimation value of the kth filtering period, N is the sliding window length, and gamma i is the innovation of the ith sampling period; the theoretical innovation covariance calculation method comprises the following steps: Wherein S k is the theoretical innovation covariance of the kth filtering period, h k is the observation matrix of the kth filtering period, P k|k-1 is the prediction error covariance of the kth filtering period, and R k is the observation noise covariance of the kth filtering period.
6. The method according to claim 1, wherein the satellite navigation health and visual navigation health calculation method is as follows: Wherein PDOP is a position accuracy attenuation factor, β, μ are adjustment parameters, N match is a matching feature point number, N total is an extraction total point number, σ reproj is a reprojection error standard deviation.
7. The method of claim 6, wherein the method of calculating satellite fusion weights and visual fusion weights from satellite navigation health and visual navigation health is as follows: Wherein omega gnss is satellite fusion weight, omega vis is vision fusion weight, epsilon= -6 ,H gnss is satellite navigation health degree, and H vis is vision navigation health degree.
8. The method of claim 1, wherein the method of calculating the weighted equivalent observation matrix and noise covariance is as follows: Wherein, the Respectively weighted equivalent observation matrix and noise covariance, wherein omega gnss is satellite fusion weight, omega vis is vision fusion weight, h gnss is satellite observation matrix, and h vis is vision observation matrix.
9. The method according to any one of claims 1 to 8, wherein the filtering estimation comprises calculating a filtering gain, performing a state update, and outputting fused position, velocity, and attitude information; The calculating the filter gain includes: where K k is the filter gain of the kth filter period, P k|k-1 is the prediction error covariance of the kth filter period, P k|k is the prediction error covariance update value of the kth filter period, Updating the value for the state vector for the kth filtering period, For the prediction state vector of the kth filtering period, gamma k is the innovation of the kth filtering period, Respectively weighted equivalent observation matrix and noise covariance.
10. An inertial/satellite/visual adaptive integrated navigation system, comprising: the integrated navigation module comprises an inertial measurement unit, a satellite receiver and a camera and is used for acquiring measurement data; The data processing module is used for calculating satellite navigation observance according to the satellite navigation data and the inertial data, constructing a satellite observation model, calculating visual navigation observance according to the visual navigation data and the inertial data and constructing a visual observation model; The self-adaptive noise estimation module is used for respectively adopting a self-adaptive covariance estimation method based on innovation aiming at the satellite observation model and the visual observation model to obtain a satellite observation noise covariance update value and a visual observation noise covariance update value; The sensor health evaluation model is used for calculating satellite navigation health and visual navigation health and calculating satellite fusion weight and visual fusion weight according to the satellite navigation health and the visual navigation health; The data updating module is used for controlling the updating of the satellite navigation observed quantity and the visual navigation observed quantity according to the satellite fusion weight and the visual fusion weight; and the filtering estimation module is used for calculating the weighted equivalent observation matrix and the noise covariance and carrying out filtering estimation.

Description

Inertial/satellite/vision self-adaptive integrated navigation method and system Technical Field The invention belongs to the technical field of navigation and positioning, and particularly relates to an inertial/satellite/vision self-adaptive combined navigation method and system. Background With the rapid development of intelligent mobile platforms, the navigation positioning capability with high precision, all weather and full scene becomes a key technical bottleneck. The single navigation system has obvious limitations that the inertial navigation system has the advantages of strong autonomy, high updating frequency, good short-term precision and the like, but the error is accumulated along with time, the long-term use precision is rapidly reduced, the global satellite navigation system such as GPS, beidou and the like can provide absolute position and speed information, but signals are easy to lose or the precision is reduced in shielding environments such as urban canyons, tunnels, forest areas and the like, and the visual navigation system can realize relative position estimation through image feature extraction and matching, has the advantages of no accumulated error and strong environment perception capability, and is easy to be influenced by illumination change, texture deletion and dynamic object interference. In the prior art, there are methods of combining inertia/satellite or inertia/vision, such as information fusion using kalman filtering or extended kalman filtering. However, the conventional filtering method generally adopts a fixed noise statistical model, and is difficult to adapt to time-varying property of error of each sensor in a complex dynamic environment, so that filtering divergence or accuracy is reduced. Furthermore, when a certain sensor fails or performance is degraded, an effective adaptive adjustment mechanism is lacking, affecting system robustness. Therefore, there is a need for an adaptive integrated navigation method capable of dynamically adjusting a fusion strategy according to environmental changes and sensor states, so as to improve stability and accuracy of the system in complex scenes. Disclosure of Invention The invention aims to solve one of the technical problems, and provides an inertial/satellite/vision self-adaptive combined navigation method, which is characterized in that a self-adaptive noise estimation and sensor health evaluation mechanism is introduced by constructing an inertial/satellite/vision three-source information fusion frame, so that the dynamic weighted fusion of multi-sensor data is realized, and the robustness and the accuracy of a navigation system under complex environments such as satellite signal lock losing, vision characteristic losing and the like are improved. The technical scheme adopted by the invention is as follows: as an aspect of the present invention, there is provided an inertial/satellite/visual adaptive integrated navigation method including the steps of: inertial measurement and navigation solution are carried out to obtain inertial data; calculating satellite navigation observance according to the satellite navigation data and the inertial data, and constructing a satellite observation model; calculating visual navigation observance according to the visual navigation data and the inertial data, and constructing a visual observation model; aiming at a satellite observation model and a visual observation model, respectively adopting an adaptive covariance estimation method based on innovation to obtain a satellite observation noise covariance update value and a visual observation noise covariance update value; Constructing satellite navigation health degree and visual navigation health degree, and calculating satellite fusion weight and visual fusion weight according to the satellite navigation health degree and the visual navigation health degree; If the satellite fusion weight is greater than the first weight threshold, updating the satellite navigation observed quantity, and entering the next step, otherwise, directly entering the next step; and calculating the weighted equivalent observation matrix and noise covariance, and carrying out filtering estimation. Further, the satellite observation model is: zgnss＝hgnssx+rgnss Wherein z gnss is satellite navigation observed quantity, at least comprises a difference value between a satellite position and an inertial position and a difference value between a satellite speed and an inertial speed, h gnss is a satellite observation matrix, r gnss is a satellite measurement noise sequence, and x is a state vector; The visual observation model is as follows: zvis＝hvisx+rvis wherein z vis is visual navigation observed quantity, at least comprises a difference value between a visual position and an inertial position and a difference value between a visual gesture and an inertial gesture, h vis is a visual observation matrix, and r vis is a visual measurement noise sequence. Further,