CN117537821A - Combined navigation positioning method of VO/INS/GPS with loose coupling EMD noise reduction - Google Patents

Abstract

The invention relates to the field of fusion of various navigation sensors, comprising a VO-vision sensor, an INS inertial navigation system and a GPS navigation system, in particular to a combined navigation positioning method of a VO/INS/GPS with loose coupling EMD noise reduction. The positioning method provided by the invention can be used for relieving the problem of limited access to the GPS signal in the environment of GPS rejection and improving the navigation positioning precision. For this purpose, the measurement model of the proposed framework is provided by two measurement sources of the GPS and VO system. In other words, the EKF update is performed by VO measurements in an environment without any GPS signals. In addition, when the system proposed by the present invention is placed in an environment where GPS can be significantly accessed, the state vector of the system can be updated by GPS and VO. In this case, the problem of a low sampling rate of the EKF update is solved and an update phase with a large number of measurements is provided.

Description

Combined navigation positioning method of VO/INS/GPS with loose coupling EMD noise reduction

Technical Field

The invention relates to the field of fusion of various navigation sensors, comprising a VO-vision sensor, an INS inertial navigation system and a GPS navigation system, in particular to a combined navigation positioning method of a VO/INS/GPS with loose coupling EMD noise reduction.

Background

In the automatic driving navigation of an automobile, the GPS possibly fails in a satellite acquisition part due to the environmental problem of the automobile, the INS inertial navigation system has the problems of drift and the like, and one of the best solutions for solving the problems of the independent navigation system and the GPS is to integrate the integrated system and the GPS with each other because the integrated system and the GPS have complementary characteristics. These methods are poor in performance because the GPS signal is blocked in the environment of GPS rejection. To solve this problem and create an accurate independent navigation system, most researchers currently use a visual measurement (VO) navigation system. The VO can estimate the position by extracting and tracking specific features, i.e. points of interest, in successive images. The VO methods can be divided into different groups according to the corresponding feature types, acceleration segment test (FAST), FAST enhanced function (SURF), center around extreme value (censer), scale Invariant Feature Transform (SIFT), FAST orientation and rotation profile (ORB), tracking thinking, camera.

While the VO navigation system has some advantages over other frameworks, it has serious problems of inconsistent feature matching between sequence images, lack of critical information in low-texture environments, and the like. Nevertheless, most scholars have taken some measure to integrate VO systems with other odometers, such as INS, radar, lasers, etc. In recent years, along with the development and innovation of technology, a plurality of sensors for solving different complex scenes appear, and meanwhile, a method for mutually compensating errors by fusion positioning among the sensors is also widely studied, wherein the combination of a visual sensor (VO) and an Inertial Navigation System (INS) is an emerging development trend in recent years, different measurement methods of different sensors are utilized to obtain different observation information, and then fusion settlement is carried out, so that the aim of mutually compensating is fulfilled. Therefore, the navigation positioning method capable of integrating the three navigation positioning systems of VO/INS/GPS is developed, and the accuracy of the INS/GPS in the GPS signal loss environment can be effectively improved, so that the navigation accuracy is effectively improved.

Disclosure of Invention

The invention provides a combined navigation positioning method of VO/INS/GPS with loose coupling EMD noise reduction for solving the navigation positioning problem under the GPS rejection environment at present, and in addition, a denoising method based on EMD empirical mode decomposition, namely an EMD-Ciit and an EMD-Hurst method, is used for denoising the measurement result of an inertial sensor before data fusion so as to improve the signal to noise ratio of the measurement of the inertial sensor.

The invention is realized by adopting the following technical scheme: a loose coupling EMD noise reduction VO/INS/GPS combined navigation positioning method adopts a visual sensor VO, a global positioning system sensor GPS and an inertial navigation system INS combined navigation; the positioning method realizes the fusion of VO/INS/GPS through a Kalman gain filtering algorithm EKF, and comprises the following steps:

(1) Constructing an IMU related state vector by using INS data;

(2) Obtaining a state vector related to the feature through preprocessing the visual data;

(3) Increasing IMU related state vectors and feature related state vectors to provide an amplified system state vector;

(4) According to whether GPS or vision data exist, a measurement vector, an augmentation matrix state vector and a measurement vector constant matrix of an INS system are established;

(5) Predicting an augmented matrix state vector and an augmented covariance matrix by:

phi in _sys (t _k ) And L _sys (t _k ) Respectively a discrete time output matrix and a discrete time input matrix, d _sys (t) represents measurement by noise reduction accelerometer and gyroscopeMeasurement vector, P, of INS system composed of magnitude _sys (t) is an augmented covariance matrix, and-sum + represents a priori and a posterior estimates, respectively, Q (t) _k ) The power spectral density of the system noise vector in the IMU;

(6) The kalman gain matrix is calculated as follows:

h in _sys For measuring vector constant matrix, representing the relation between the state vector of the augmentation matrix and the measuring vector of the INS system, R is covariance matrix of measuring noise;

(7) Updating the augmented matrix state vector and the augmented covariance matrix available for measurement:

z in _sys (t) measurement model of denoised VO/INS/GPS

Z _sys (t)＝[Z _G (t),Z _C (t)] ^T (6)

Z in _G (t) is the GPS subsystem measurement, which is a 1*6 vector containing the vehicle position and velocity in the coordinate system, Z _C (t) normalized pixel coordinates provided for the vision sensor;

(8) Returning to the step (1).

The integration method provided by the invention can relieve the problem of limited access to the GPS signal in the environment of GPS rejection and improve the navigation positioning precision. For this purpose, the measurement model of the proposed framework is provided by two measurement sources of the GPS and VO system. In other words, the EKF update is performed by VO measurements in an environment without any GPS signals. In addition, when the system proposed by the present invention is placed in an environment where GPS can be significantly accessed, the state vector of the system can be updated by GPS and VO. In this case, the problem of a low sampling rate of the EKF update is solved and an update phase with a large number of measurements is provided.

Further, the INS data in step (1) needs to be preprocessed, which includes the following steps: (1) taking output signals of inertial sensors of the accelerometer and the gyroscope as original signals, and performing EMD empirical mode decomposition operation on the original signals to obtain an eigenmode function set IMFs after the original signals are decomposed;

(2) carrying out denoising treatment on IMFs self-adaptive selection EMD-Ciit and EMD-Hurst methods of original signals;

(3) and obtaining the denoised original signal and solving a mechanical equation required by navigation.

To improve the signal-to-noise ratio of inertial measurements, which are typically contaminated with high frequency noise, EMDs based on clear iteration interval threshold (EMD CIIT) versions and EMD Hurst exponential versions are applied prior to data fusion. The invention can alleviate the problem of limited access to GPS signals in the environment of GPS rejection by the above-proposed integration method.

The invention discloses a fusion positioning algorithm based on EMD noise reduction to solve measured information under different conditions so as to achieve the purpose of coping with complexity of navigation scenes. And then, performing multi-element fusion on the INS data set subjected to noise reduction optimization and the acquired data of the GNSS by utilizing the feature vector obtained by resolving through an Extended Kalman Filter (EKF) algorithm, and finally outputting a result to achieve the purpose of improving the navigation precision in a GPS refusing environment.

Drawings

FIG. 1EMD-Hurst block diagram.

FIG. 2 is a block diagram of the EMD-VO/INS/GPS system.

Detailed Description

The method mainly comprises two aspects, namely, a preprocessing operation of denoising an inertial measurement result by using a proposed EMD loose coupling empirical mode decomposition method, wherein the denoising operation of accelerometer and gyroscope output is included in the process; secondly, a measurement model of the IO/INS/GPS framework under the environment without GPS and with VO and GPS measurement is provided, and finally the required high-precision positioning result is obtained through fusion.

1. Preprocessing of the results of inertial measurements to denoise

An important problem that leads to reduced accuracy of inertial navigation systems is the presence of high and low frequency noise in the sensor, both of which can have several adverse effects on the output of the inertial sensor. In order to eliminate noise from the inertial sensor output, there are many ways in which to consider overcoming these problems. Such as discrete wavelet transforms, are one of the effective methods of improving the signal-to-noise ratio of inertial sensors. Because the wavelet transformation adopts a fixed basis function, the denoising effect on the inertial sensor is poor. To solve this problem, EMD is selected in the face of denoising of the inertial sensor because it can adaptively derive the basis function from the raw signal.

The specific method comprises the following steps:

(1) taking output signals of inertial sensors of the accelerometer and the gyroscope as original signals, and performing EMD empirical mode decomposition operation on the original signals to obtain IMFs after decomposing the original signals;

(2) carrying out denoising treatment on IMFs self-adaptive selection EMD-Ciit and EMD-Hurst methods of original signals;

(3) and obtaining a denoised original signal to be used for solving a mechanical equation required by navigation.

The mechanical equations are conventional in the art.

The EMD-Hurst method has lower computational complexity and accuracy because the computational complexity and accuracy of the EMD-Hurst is different than that of the EMD-CIIT. Therefore, the invention can select one of two denoising methods to be used when the output signals of the inertial sensors of the accelerometer and the gyroscope are subjected to noise reduction processing according to the precision and the calculation force resources required in real time.

Wherein the EMD-Ciit method is as follows:

(1) EMD (empirical mode decomposition) is carried out on the voice with noise to obtain IMFs components; carrying out traditional threshold denoising on the signal component and obtaining a denoised IMFs component;

(2) estimating the noise of the first IMFs component in the original signal to obtain an estimated value of the noise; reconstructing the remaining IMFs component and the processed IMF component, and obtaining a reconstruction signal; randomly varying the noise estimate in the first IMFs;

(3) adding the noise estimation after random change with the reconstruction signal to obtain a signal different from the original noise signal, and then carrying out EMD decomposition; and (3) carrying out thresholding treatment on the decomposed IMF components, then carrying out signal reconstruction, repeating the operation of the step (2) on the rest signal components to obtain data after each treatment, and finally, averaging to obtain the mean value of the original signal after denoising.

The EMD-Hurst method comprises the following steps:

as shown in fig. 1, when the original signal reaches a block of the EMD, its IMF is calculated and sent to the next block to estimate the hurst value of the IMF. A hurst value between 0 and 0.5 is characteristic of anti-persistence behavior, the closer the value is to 0, the more the time series tends to return to its long-term average. Thus, if the hurst of each IMF does not exceed a predefined threshold (typically 0.5), it is treated as a noise component and eliminated from the original signal.

2. Fusion algorithm implementation

As shown in fig. 2, the method of the present invention comprises three important parts, i.e., VO, GPS and INS systems, respectively. The first part and the second part, i.e. VO and GPS, give a measurement model of the proposed method. As previously described, the GPS system prepares measurement values (zg) through acquisition, tracking, and navigation operations, respectively. In addition, some post-operation VO measurements are also provided. For this purpose, first a stereoscopic frame (I _k,L ,I _k,R ) No distortion correction is done, as these activities can help to improve other VO parts, which are common in VO navigation. The third part of the proposed integration method, INS, provides the values of the process model. The raw signals of the accelerometer and gyroscope of the INS system are denoised by the EMD-CIIT and EMD-Hurst methods before they are injected into the process model, so if the denoised signals of the accelerometer and gyroscope have a high signal to noise ratio, they are selected and sent to the block of mechanized equations. As shown in fig. 2, all described parts are finally collectedInto loosely coupled EKF blocks.

The core of the system is an EKF algorithm, which has two important parts, namely a filter prediction part and a filter updating part, wherein the first part predicts an augmented state vector and an augmented covariance matrix, and the second part updates a predicted value through measurement data of a GPS or vision sensor subsystem. Therefore, the EKF algorithm of the present invention is designed as follows:

(1) Constructing an IMU related state vector by using INS data;

(2) Obtaining a state vector related to the feature through preprocessing the visual data;

(3) Increasing IMU related state vectors and feature related state vectors to provide an amplified system state vector;

(4) According to whether GPS or vision data exist, a measurement vector, an augmentation state vector and a measurement vector constant matrix are established;

(5) The augmented state vector and the augmented covariance matrix are predicted by:

phi in _sys (t _k ) And L _sys (t _k ) Respectively a discrete time output matrix and a discrete time input matrix, d _sys (t) represents an INS system measurement vector consisting of noise reduction accelerometer and gyroscope measurements, P _sys (t) is an augmented covariance matrix, and-sum + represents a priori and a posterior estimates, respectively, Q (t) _k ) The power spectral density of the system noise vector in the IMU;

(6) The kalman gain matrix is calculated as follows:

in the middle ofH _sys For measuring vector constant matrix, representing the relation between the state vector of the augmentation matrix and the measuring vector of the INS system, R is covariance matrix of measuring noise;

(7) Updating the augmented matrix state vector and the augmented covariance matrix available for measurement:

z in _sys (t) measurement model of denoised VO/INS/GPS

Z _sys (t)＝[Z _G (t),Z _C (t)] ^T (6)

Z in _G (t) is the GPS subsystem measurement, which is a 1*6 vector containing the vehicle position and velocity in the coordinate system, Z _C (t) normalized pixel coordinates provided for the vision sensor;

(8) Returning to the step (1).

The invention provides a VO/INS/GPS integration method based on EMD loose coupling noise reduction, which is used for improving the precision of an INS/GPS system in a GPS signal loss environment. For this purpose, the invention uses a new EKF measurement model. In this model, the measurements to update the system state vector in the EKF are provided by the GPS and VO system, so the update phase of the EKF is performed by VO measurements during GPS signal loss of lock. Meanwhile, the invention uses a stereo camera in the VO system to overcome the problem of scale blurring. And the EMD-CIIT and EMD-Hurst methods are adopted to perform noise reduction treatment on the output signals of the inertial sensors of the accelerometer and the gyroscope in the INS system so as to improve the signal-to-noise ratio of the inertial sensors and simultaneously keep the important information of the motion in the original signals.

Claims (5)

1. A loose coupling EMD noise reduction VO/INS/GPS combined navigation positioning method adopts a visual sensor VO, a global positioning system sensor GPS and an inertial navigation system INS combined navigation; the method is characterized in that the positioning method realizes the fusion of VO/INS/GPS through a Kalman gain filtering algorithm EKF, and comprises the following steps: (1) constructing an IMU related state vector by using INS data;

(2) Obtaining a state vector related to the feature through preprocessing the visual data;

(3) Increasing IMU related state vectors and feature related state vectors to provide an amplified system state vector;

(4) According to whether GPS or vision data exist, an INS system measurement vector, an augmentation matrix state vector and a measurement vector constant matrix are established;

(5) Predicting an augmented matrix state vector and an augmented covariance matrix by:

phi in _sys (t _k ) And L _sys (t _k ) Respectively a discrete time output matrix and a discrete time input matrix, d _sys (t) represents the measurement vector, P, of an INS system consisting of noise-reducing accelerometer and gyroscope measurements _sys (t) is an augmented covariance matrix, and-sum + represents a priori and a posterior estimates, respectively, Q (t) _k ) The power spectral density of the system noise vector in the IMU;

(6) The kalman gain matrix is calculated as follows:

h in _sys For measuring vector constant matrix, representing the relation between the state vector of the augmentation matrix and the measuring vector of the INS system, R is covariance matrix of measuring noise;

(7) Updating the augmented matrix state vector and the augmented covariance matrix available for measurement:

z in _sys (t) measurement model of denoised VO/INS/GPS

Z _sys (t)＝[Z _G (t),Z _C (t)] ^T (6)

Z in _G (t) is the GPS subsystem measurement, which is a 1*6 vector containing the vehicle position and velocity in the coordinate system, Z _C (t) normalized pixel coordinates provided for the vision sensor;

(8) Returning to the step (1).

2. The integrated navigation positioning method of VO/INS/GPS with loose coupling EMD noise reduction according to claim 1, wherein INS data in step (1) is required to be preprocessed, comprising the following steps: (1) taking output signals of inertial sensors of the accelerometer and the gyroscope as original signals, and performing EMD empirical mode decomposition operation on the original signals to obtain an eigenmode function set IMFs after the original signals are decomposed;

(2) carrying out denoising treatment on IMFs self-adaptive selection EMD-Ciit and EMD-Hurst methods of original signals;

(3) and obtaining the denoised original signal and solving a mechanical equation required by navigation.

3. The integrated VO/INS/GPS navigation positioning method with loose coupling EMD noise reduction according to claim 2, wherein the EMD-Ciit method is as follows:

(1) EMD (empirical mode decomposition) is carried out on the voice with noise to obtain IMFs components; carrying out traditional threshold denoising on the signal component and obtaining a denoised IMFs component;

(2) estimating the noise of the first IMFs component in the original signal to obtain an estimated value of the noise; reconstructing the remaining IMFs component and the processed IMF component, and obtaining a reconstruction signal; randomly varying the noise estimate in the first IMFs;

(3) adding the noise estimation after random change with the reconstruction signal to obtain a signal different from the original noise signal, and then carrying out EMD decomposition; and (3) carrying out thresholding treatment on the decomposed IMF components, then carrying out signal reconstruction, repeating the operation of the step (2) on the rest signal components to obtain data after each treatment, and finally, averaging to obtain the mean value of the original signal after denoising.

4. The integrated navigation positioning method of VO/INS/GPS with loose coupling EMD noise reduction according to claim 2, wherein the EMD-Hurst method is as follows: when the original signal reaches the block of the EMD, calculating the IMF thereof and transmitting the IMF to the next block to estimate the Hurst value of the IMF; a hurst value between 0 and 0.5 is characteristic of anti-persistence behavior, the closer the value is to 0, the more the time series tends to return to its long-term average; thus, if the hurst of each IMF does not exceed the predefined threshold, it is treated as a noise component and eliminated from the original signal.

5. The VO/INS/GPS combined navigation positioning method with loose coupling EMD noise reduction according to claim 4, wherein the predefined threshold is 0.5 in the EMD-Hurst method.