阅读说明：本技术 一种组合导航系统及方法 (Combined navigation system and method ) 是由 曹宜 张杨勇 梅雪松 王琦思 刘谋荣 刘庆 于 2021-10-28 设计创作，主要内容包括：本发明公开了一种组合导航系统及方法。该系统包括：无线电导航组件、惯导组件、磁力计和计算模块；所述计算模块用于求解载体当前的姿态、位置与地球参考物理量之间的最小二乘解,利用磁力计等传感器稳定性好,高频噪声低的特点,降低无线电导航组件定位误差,同时可以校准其它惯导组件的零点偏移。本发明不依赖GNSS系统,具有运算量小、解算精度高、测量准确、稳定性好的特点。(The invention discloses a combined navigation system and a method. The system comprises: the system comprises a radio navigation component, an inertial navigation component, a magnetometer and a calculation module; the calculation module is used for solving a least square solution between the current attitude and position of the carrier and the georeferenced physical quantity, and the positioning error of the radio navigation assembly is reduced by utilizing the characteristics of good stability and low high-frequency noise of sensors such as a magnetometer and the like, and meanwhile, the zero offset of other inertial navigation assemblies can be calibrated. The method does not depend on a GNSS system, and has the characteristics of small calculation amount, high resolving precision, accurate measurement and good stability.)

1. A combined navigation system, comprising:

the system comprises a radio navigation component, an inertial navigation component, a magnetometer and a calculation module;

the calculation module is configured to perform the following steps at each time step:

determining a carrier attitude predicted value and a carrier position predicted value of the current time step according to the measurement data of the inertial navigation component in the time interval from the previous time step to the current time step, and the carrier attitude predicted value and the carrier position predicted value of the previous time step;

calculating a first error between the measurement data of the magnetometer at the current time step and the carrier attitude predicted value at the current time step, calculating a second error between the measurement data of the inertial navigation component at the current time step and the carrier attitude predicted value at the current time step, calculating a third error between the carrier position predicted value at the current time step and the measurement data of the radio navigation component at the current time step, and correcting the carrier attitude predicted value and the carrier position predicted value at the current time step according to the first error, the second error and the third error by adopting a nonlinear least square algorithm.

2. The integrated navigation system according to claim 1, wherein said calculating a first error between said magnetometer measurements at a current time step and a predicted vehicle attitude at the current time step comprises the steps of:

determining a predicted measurement value of the magnetometer at the current time step according to the predicted carrier attitude value at the current time step;

and calculating the difference between the predicted measurement value of the magnetometer at the current time step and the measurement data of the magnetometer at the current time step as a first error.

3. The integrated navigation system of claim 1, wherein the inertial navigation assembly includes a gyroscope and an accelerometer.

4. The integrated navigation system of claim 3, wherein calculating a second error between the measured data of the inertial navigation component at the current time step and the predicted value of the attitude of the carrier at the current time step comprises the steps of:

determining a predicted measurement value of the accelerometer at the current time step according to the predicted value of the carrier attitude at the current time step;

and calculating the difference between the predicted measurement value of the accelerometer at the current time step and the measurement data of the accelerometer at the current time step as a second error.

5. The integrated navigation system of claim 1, wherein the computing module is configured to perform the following steps at each time step:

and correcting errors of the inertial navigation assembly.

6. The integrated navigation system of claim 1, wherein the radio navigation module is electrically connected to the computing module via an RS422 interface, the inertial navigation module is electrically connected to the computing module via an RS422 interface, and the magnetometer is electrically connected to the computing module via an I2C interface.

7. The integrated navigation system of, further comprising an upper computer electrically connected to the computing module.

8. A combined navigation method, comprising the steps of:

respectively adopting a radio navigation component, an inertial navigation component and a magnetometer which are arranged on a carrier to carry out measurement;

at each time step the following steps are performed:

determining a carrier attitude predicted value and a carrier position predicted value of the current time step according to the measurement data of the inertial navigation component in the time interval from the previous time step to the current time step, and the carrier attitude predicted value and the carrier position predicted value of the previous time step;

calculating a first error between the measurement data of the magnetometer at the current time step and the carrier attitude predicted value at the current time step, calculating a second error between the measurement data of the inertial navigation component at the current time step and the carrier attitude predicted value at the current time step, calculating a third error between the carrier position predicted value at the current time step and the measurement data of the radio navigation component at the current time step, and correcting the carrier attitude predicted value and the carrier position predicted value at the current time step according to the first error, the second error and the third error by adopting a nonlinear least square algorithm.

9. The integrated navigation method according to claim 8, wherein said calculating a first error between said magnetometer measurements at a current time step and a predicted vehicle attitude at the current time step comprises the steps of:

determining a predicted measurement value of the magnetometer at the current time step according to the predicted carrier attitude value at the current time step;

and calculating the difference between the predicted measurement value of the magnetometer at the current time step and the measurement data of the magnetometer at the current time step as a first error.

10. The integrated navigation method of claim 8, wherein the inertial navigation module includes a gyroscope and an accelerometer, and wherein calculating a second error between the measurement data of the inertial navigation module at the current time step and the predicted value of the attitude of the carrier at the current time step includes the steps of:

determining a predicted measurement value of the accelerometer at the current time step according to the predicted value of the carrier attitude at the current time step;

and calculating the difference between the predicted measurement value of the accelerometer at the current time step and the measurement data of the accelerometer at the current time step as a second error.

Technical Field

The invention belongs to the technical field of navigation, and particularly relates to a combined navigation system and method.

Background

GNSS occupies a core position in a modern positioning, navigation and time service PNT system, and has obvious comprehensive advantages in coverage, precision and use cost compared with other navigation systems. However, due to the weakness and the disadvantage of easy interference, the PNT system which relies heavily on GNSS has great risks in terms of social economic safety, production safety and defense safety, and therefore, a navigation system which does not rely on GNSS is required to be established.

Disclosure of Invention

Aiming at least one defect or improvement requirement in the prior art, the invention provides a combined navigation system and a method, which do not depend on a GNSS system and have the characteristics of small operand, high resolving precision, accurate measurement and good stability.

To achieve the above object, according to a first aspect of the present invention, there is provided a combined navigation system including:

the system comprises a radio navigation component, an inertial navigation component, a magnetometer and a calculation module;

the calculation module is configured to perform the following steps at each time step:

determining a carrier attitude predicted value and a carrier position predicted value of the current time step according to the measurement data of the inertial navigation component in the time interval from the previous time step to the current time step, and the carrier attitude predicted value and the carrier position predicted value of the previous time step;

calculating a first error between the measurement data of the magnetometer at the current time step and the carrier attitude predicted value at the current time step, calculating a second error between the measurement data of the inertial navigation component at the current time step and the carrier attitude predicted value at the current time step, calculating a third error between the carrier position predicted value at the current time step and the measurement data of the radio navigation component at the current time step, and correcting the carrier attitude predicted value and the carrier position predicted value at the current time step according to the first error, the second error and the third error by adopting a nonlinear least square algorithm.

Further, said calculating a first error of said magnetometer's measurement data at the current time step and a predicted carrier attitude at the current time step comprises the steps of:

determining a predicted measurement value of the magnetometer at the current time step according to the predicted carrier attitude value at the current time step;

and calculating the difference between the predicted measurement value of the magnetometer at the current time step and the measurement data of the magnetometer at the current time step as a first error.

Further, the inertial navigation assembly includes a gyroscope and an accelerometer.

Further, calculating a second error between the measurement data of the inertial navigation component at the current time step and the predicted value of the attitude of the carrier at the current time step comprises the following steps:

determining a predicted measurement value of the accelerometer at the current time step according to the predicted value of the carrier attitude at the current time step;

and calculating the difference between the predicted measurement value of the accelerometer at the current time step and the measurement data of the accelerometer at the current time step as a second error.

Further, the calculation module is configured to perform the following steps at each time step:

and correcting errors of the inertial navigation assembly.

Further, the radio navigation assembly is electrically connected with the computing module through an RS422 interface, the inertial navigation assembly is electrically connected with the computing module through an RS422 interface, and the magnetometer is electrically connected with the computing module through an I2C interface.

Further, the device also comprises an upper computer electrically connected with the computing module.

According to a second aspect of the present invention, there is provided a combined navigation method, comprising the steps of:

respectively adopting a radio navigation component, an inertial navigation component and a magnetometer which are arranged on a carrier to carry out measurement;

at each time step the following steps are performed:

determining a carrier attitude predicted value and a carrier position predicted value of the current time step according to the measurement data of the inertial navigation component in the time interval from the previous time step to the current time step, and the carrier attitude predicted value and the carrier position predicted value of the previous time step;

calculating a first error between the measurement data of the magnetometer at the current time step and the carrier attitude predicted value at the current time step, calculating a second error between the measurement data of the inertial navigation component at the current time step and the carrier attitude predicted value at the current time step, calculating a third error between the carrier position predicted value at the current time step and the measurement data of the radio navigation component at the current time step, and correcting the carrier attitude predicted value and the carrier position predicted value at the current time step according to the first error, the second error and the third error by adopting a nonlinear least square algorithm.

Further, said calculating a first error of said magnetometer's measurement data at the current time step and a predicted carrier attitude at the current time step comprises the steps of:

determining a predicted measurement value of the magnetometer at the current time step according to the predicted carrier attitude value at the current time step;

and calculating the difference between the predicted measurement value of the magnetometer at the current time step and the measurement data of the magnetometer at the current time step as a first error.

Further, the inertial navigation component comprises a gyroscope and an accelerometer, and calculating a second error between the measurement data of the inertial navigation component at the current time step and the predicted value of the attitude of the carrier at the current time step comprises the following steps:

determining a predicted measurement value of the accelerometer at the current time step according to the predicted value of the carrier attitude at the current time step;

and calculating the difference between the predicted measurement value of the accelerometer at the current time step and the measurement data of the accelerometer at the current time step as a second error.

In general, compared with the prior art, the invention has the following beneficial effects: through the fusion of multiple sensor data, the redundancy of information is improved, namely, the variable quantity of the current position is estimated through the extra attitude and speed information relative to the radio navigation assembly, because the magnetometer system has excellent short-term stability, the positioning error of the radio navigation assembly caused by noise interference can be effectively inhibited, the radio navigation assembly can replace a GNSS positioning system to become a reliable high-precision navigation positioning means, the penetration force is strong, the radio navigation assembly is not easily interfered by people intentionally, the radio navigation assembly has higher application value in the fields with higher requirements on safety, such as military fields, economic fields and the like, and the calculation speed is greatly improved relative to the traditional Kalman filtering algorithm.

Drawings

FIG. 1 is a schematic structural diagram of a combined navigation system according to an embodiment of the present invention;

fig. 2 is a schematic diagram of a computing principle of a computing module according to an embodiment of the present invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. In addition, the technical features involved in the embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.

The radio Navigation assembly (eLoran Navigation System) originates from the period of world war II, the effective range of the radio Navigation assembly (eLoran Navigation System) can reach more than 2000km, and the radio Navigation assembly is regarded as the best substitute scheme of a GNSS System, therefore, the embodiment of the invention adopts an eLoran Navigation receiver, an Inertial Navigation assembly (INS module for short) and a magnetometer to perform data fusion by using a nonlinear minimum two-component method, and the aims of improving Navigation precision and reducing calculated amount are achieved by making good for each other through sensors.

As shown in fig. 1, a combined navigation system according to an embodiment of the present invention includes: radio navigation subassembly, inertial navigation subassembly, magnetometer and calculation module.

Furthermore, the integrated navigation system also comprises an upper computer electrically connected with the computing module.

Furthermore, the integrated navigation system also comprises a power supply module which is responsible for providing 5.5V voltage for the radio navigation assembly, the inertial navigation assembly, the magnetometer and the calculation module.

Further, the inertial navigation assembly includes a gyroscope and an accelerometer.

In one embodiment, the computing module employs a micro-computing unit; the radio navigation component adopts a Beidou-eLoran navigation receiver provided by Henan Shang Yu technology, is connected with the micro-computing unit through RS422 and is communicated with an ARM module of the micro-computing unit through a communication conversion module; the INS module adopts an HZ-1 type optical fiber inertia measurement system of the company of the red peaks of three rivers in Hubei, is connected with the micro-computing unit through an RS422, and is communicated with an ARM module of the micro-computing unit through a communication conversion module. The magnetometer adopts an RM3100 three-axis magnetic field sensor of PNI company, and communicates with an ARM chip in an SPI mode, and the noise RMS is 15 nT. The sampling rate of the micro-computing unit is 10Hz, and after data of eLoran/INS/magnetometer are acquired, the current attitude and position are computed by using a data fusion algorithm based on nonlinear least square estimation.

Further, the principle of the calculation module is shown in fig. 2, which is used to perform the following steps at each time step:

and S1, determining the carrier attitude predicted value and the carrier position predicted value of the current time step according to the measurement data of the inertial navigation component in the time interval from the previous time step to the current time step, and the carrier attitude predicted value and the carrier position predicted value of the previous time step.

Specifically, firstly, a carrier attitude prediction value is obtained according to gyroscope data:

wherein:to utilize a gyroscopeThe estimated attitude prediction value of the carrier at the time step t by the spirometer,the predicted value of the carrier attitude at the last time step t-1,measured by a gyroscope at time steps t-1-t, Δ t is the time step interval,representing a direct product operation of a quaternion.

The velocity calculated using the accelerometer data can then be used to derive an estimate of the position:

wherein the content of the first and second substances,for the measurement data of the accelerometer at time steps t-1 to t,as an estimate of the velocity at this time step t,is the predicted value of the carrier position at this time step t,and (4) representing the predicted value of the carrier position in the time step t-1 by a conjugate quaternion.

And S2, calculating a first error between the measurement data of the magnetometer at the current time step and the predicted carrier attitude value at the current time step, calculating a second error between the measurement data of the inertial navigation component at the current time step and the predicted carrier attitude value at the current time step, calculating a third error between the predicted carrier position value at the current time step and the measurement data of the radio navigation component at the current time step, and correcting the predicted carrier attitude value and the predicted carrier position value at the current time step according to the first error, the second error and the third error by adopting a nonlinear least square algorithm.

Further, said calculating a first error of said magnetometer's measurement data at the current time step and a predicted carrier attitude at the current time step comprises the steps of:

(1) determining a predicted measurement value of the magnetometer at the current time step according to the predicted carrier attitude value at the current time step;

(2) and calculating the difference between the predicted measurement value of the magnetometer at the current time step and the measurement data of the magnetometer at the current time step as a first error.

Further, calculating a second error between the measurement data of the inertial navigation component at the current time step and the predicted value of the attitude of the carrier at the current time step comprises the following steps:

(1) determining a predicted measurement value of the accelerometer at the current time step according to the predicted value of the carrier attitude at the current time step;

(2) and calculating the difference between the predicted measurement value of the accelerometer at the current time step and the measurement data of the accelerometer at the current time step as the second error.

In particular, a gravity vector is reversely deduced according to a predicted value of the attitude of the carrierAnd the earth magnetic field vectorRepresentation in the carrier coordinate system and actual measurement value of the accelerometer in the carrier coordinate systemAnd magnetometer actual measurementsThe error between:

in the formula (4), the first and second groups,and the difference between the predicted measurement value of the accelerometer at the current time step and the measurement data of the accelerometer at the current time step is the second error.The difference between the predicted measurement of the magnetometer at the previous time step and the measurement data of the magnetometer at the current time step, i.e., the first error described above.

Jacobian's J_g，J_mComprises the following steps:

the third error may be calculated by:

in the formula (6), the first and second groups,carrier position prediction for current time stepMeasurement data of the radio navigation module with the current time stepI.e. the third error described above.Jacobian's J_pComprises the following steps:

the overall error function of these three errors can be constructed as:

the Jacobian determinant of the above formula is:

and then, by utilizing a nonlinear least square algorithm and iteration, the carrier attitude with the minimum error value is solved, and the current carrier attitude can be obtained. In order to improve the iteration efficiency, the iteration quantity is calculated by adopting a Gauss-Newton method:

andrespectively representing the iteration quantities of the position and the attitude, and the complete recursion formula is as follows:

wherein the content of the first and second substances,the attitude and the position of the carrier are taken as the attitude and the position of the carrier,in order to estimate the attitude of the carrier,for the amount of iteration, μ is the convergence step factor.

Further, the calculation module is configured to perform the following steps at each time step:

and S3, carrying out error correction on the inertial navigation assembly.

The accelerometer and gyroscope calculations of the carrier state have cumulative errors and therefore require calibration from position and attitude data. And calculating the self zero point offset of the INS system by using the error between the predicted attitude and position and the measured attitude position in a low-pass filtering mode.

Since the zero offset error of the gyroscope is a slow variable, it can be obtained by accumulating the error between the estimated attitude and the measured attitude:

in the formula (7), the first and second groups,the offset that represents the speed is such that,a conjugate quaternion representing the attitude at the previous time,the estimated amount of the gyroscope output obtained by the inverse estimation of the change in the attitude in a single operation is represented, and γ is an integration constant. Similarly, the bias in velocity can be obtained by a similar method:

in the formula (8), the first and second groups,the offset that represents the speed is such that,an estimate representing the velocity derived from the inverse of the change in position in a single operation, δ being an integration constant.

Thus, the angular velocity and acceleration after calibration are respectively:

in the formula (9), the reaction mixture,which represents the angular velocity after calibration and,indicating the calibrated velocity.

The micro-computing unit is communicated with the upper computer through an RS422 interface, and after receiving an instruction of 'starting work' sent by the upper computer, the micro-computing unit sends the calculated attitude and position information to the upper computer, and the attitude and position information is stored and presented by the upper computer.

The combined navigation method of the embodiment of the invention comprises the following steps:

respectively adopting a radio navigation component, an inertial navigation component and a magnetometer which are arranged on a carrier to carry out measurement;

at each time step the following steps are performed:

determining a carrier attitude predicted value and a carrier position predicted value of the current time step according to the measurement data of the inertial navigation component in the time interval from the previous time step to the current time step, and the carrier attitude predicted value and the carrier position predicted value of the previous time step;

calculating a first error between the measurement data of the magnetometer at the current time step and the carrier attitude predicted value at the current time step, calculating a second error between the measurement data of the inertial navigation component at the current time step and the carrier attitude predicted value at the current time step, calculating a third error between the carrier position predicted value at the current time step and the measurement data of the radio navigation component at the current time step, and correcting the carrier attitude predicted value and the carrier position predicted value at the current time step according to the first error, the second error and the third error by adopting a nonlinear least square algorithm.

Further, said calculating a first error of said magnetometer's measurement data at the current time step and a predicted carrier attitude at the current time step comprises the steps of:

determining a predicted measurement value of the magnetometer at the current time step according to the predicted carrier attitude value at the current time step;

and calculating the difference between the predicted measurement value of the magnetometer at the current time step and the measurement data of the magnetometer at the current time step as a first error.

Further, the inertial navigation component comprises a gyroscope and an accelerometer, and calculating a second error between the measurement data of the inertial navigation component at the current time step and the predicted value of the attitude of the carrier at the current time step comprises the following steps:

determining a predicted measurement value of the accelerometer at the current time step according to the predicted value of the carrier attitude at the current time step;

and calculating the difference between the predicted measurement value of the accelerometer at the current time step and the measurement data of the accelerometer at the current time step as a second error.

The implementation principle and technical effect of the method are similar to those of the system, and are not described herein again.

It must be noted that in any of the above embodiments, the methods are not necessarily executed in order of sequence number, and as long as it cannot be assumed from the execution logic that they are necessarily executed in a certain order, it means that they can be executed in any other possible order.

It will be understood by those skilled in the art that the foregoing is only a preferred embodiment of the present invention, and is not intended to limit the invention, and that any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the scope of the present invention.