CN113639754B - Combined navigation method based on multi-period secondary fusion EKF algorithm - Google Patents
Combined navigation method based on multi-period secondary fusion EKF algorithm Download PDFInfo
- Publication number
- CN113639754B CN113639754B CN202110918040.4A CN202110918040A CN113639754B CN 113639754 B CN113639754 B CN 113639754B CN 202110918040 A CN202110918040 A CN 202110918040A CN 113639754 B CN113639754 B CN 113639754B
- Authority
- CN
- China
- Prior art keywords
- data
- updating
- estimation
- ekf algorithm
- data updating
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Theoretical Computer Science (AREA)
- Bioinformatics & Cheminformatics (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Computation (AREA)
- Evolutionary Biology (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Bioinformatics & Computational Biology (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
The invention provides a combined navigation method based on a multicycle secondary fusion EKF algorithm, which comprises the following steps: step 100, setting a data updating period T = T, and performing data prediction and updating at each time point by adopting an EKF algorithm to obtain an estimation of a state parameter; step 200, setting a data updating period T =2t,3t,5t ⋯, extracting partial data from the output data of the actual sensors respectively, and obtaining multiple estimates of state parameters by adopting an EKF algorithm; and step 300, screening or averaging the estimation results in different periods at the common multiple time of all the data updating periods, taking the obtained value as an effective estimation value of the current time, and updating the state estimation results in different periods. The invention improves the anti-interference capability of the integrated navigation system, so that the integrated navigation system can reduce the influence of abnormal data in the data processing process.
Description
Technical Field
The invention belongs to the technical field of navigation control, and particularly relates to a combined navigation method based on a multi-period secondary fusion EKF algorithm.
Background
In recent years, the integrated navigation system is more and more concerned by people, and an intelligent carrier represented by unmanned boats, unmanned planes, unmanned vehicles and mobile robots highly depends on precise position information when performing autonomous movement, and the autonomous navigation system is the basis and the core of environment perception and decision control. Along with the more refined operation of the intelligent carrier, the faced task environment is more and more complex, the precision requirement on the navigation system is higher and higher, the single device used by the early navigation system has limitations in the aspects of precision, system stability and the like, and an ideal navigation effect is difficult to obtain, so that the combined navigation becomes a common practice.
Kalman filtering is widely applied to engineering projects of navigation systems due to high applicability and strong filtering effect of Kalman filtering in navigation calculation, and most of the existing filtering methods of the integrated navigation systems are based on Kalman filtering algorithm. In practical engineering application, a combined navigation system needs to face a complex and changeable environment, abnormal values of output data of sensors may occur due to various interferences, and in addition, due to different working principles, the update periods of various sensor data are inconsistent, and certain difficulty is brought to the combined navigation data fusion. The influence of sensor data abnormity on the combined navigation effect is not fully considered in the conventional combined navigation method.
Disclosure of Invention
In view of the above, the main objective of the present invention is to provide a combined navigation method to overcome the above-mentioned drawbacks in the prior art, comprising the following steps:
and step 300, performing secondary fusion on the estimation results in different cycle states at the time of common multiple of all data updating cycles, namely screening or averaging the estimation results obtained in all cycles, and taking the obtained value as an effective estimation value of the current time of all cycles.
Further, the integrated navigation system in step 100 is constructed based on the EKF algorithm, and the data update period t is adjusted according to the actual performance and the data update rate of the integrated navigation system.
Further, the data update period in step 200 may be any multiple of t, and may be adjusted according to the performance of the actual sensor and the computer.
Further, the estimation results in different cycle states are filtered or averaged in step 300, for example, the estimation results in different cycle states are processed by an averaging or weighted average method, and the estimation results in different cycle states can be processed in any manner known to those skilled in the art.
Further, in step 300, the second-order fusion result obtained at the common multiple time of all data updating periods is used as an effective estimation value of the current time of all data updating periods for subsequent data estimation and updating.
Furthermore, the navigation result finally output by the navigation system is taken from the updating result with the data updating period being t, and the updating results of other periods (2t, 3t,5t …) are used for providing the data for secondary fusion so as to reduce the interference of abnormal data.
The novel integrated navigation method provided by the invention has the following advantages:
the method of the invention can be flexibly deployed and adjusted according to the actual performance of the navigation equipment (for example, the number and the combination mode of data update cycles can be adjusted according to the performance of a navigation computer), and can be conveniently reconstructed on the basis of the original navigation system; the adopted multicycle secondary fusion scheme can effectively reduce the influence of sensor data abnormal values on the combined navigation result in the data fusion process, improves the anti-interference capability of the system, and has strong robustness in different environments.
Drawings
FIG. 1 is an example of a multi-cycle quadratic fusion EKF algorithm operation;
FIG. 2 is a block diagram of a combined navigation system according to an embodiment of the present invention;
FIG. 3 is a Simulink simulation structure diagram according to an embodiment of the present invention;
FIG. 4 is a comparison graph of position error (inertial navigation output position) of simulation results according to an embodiment of the present invention;
FIG. 5 is a comparison graph of position error (GPS satellite compass output position) of simulation results according to an embodiment of the present invention;
FIG. 6 is a velocity error comparison plot of simulation results for an embodiment of the present invention;
FIG. 7 is a plot of attitude error versus simulation results for 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 will be described in further detail with reference to the accompanying drawings. This description is made by way of example and not limitation to specific embodiments consistent with the principles of the invention, the description being in sufficient detail to enable those skilled in the art to practice the invention, other embodiments may be utilized and the structure of various elements may be changed and/or substituted without departing from the scope and spirit of the invention. The following detailed description is, therefore, not to be taken in a limiting sense.
First, abbreviations and explanations used in the present invention are given:
an IMU: an Inertial Measurement Unit, an Inertial Measurement Unit;
GPS: global Positioning System (gps);
FIG. 1 is an example of the operation of a multi-cycle quadratic fusion EKF algorithm. As shown in fig. 1, the basic idea of the algorithm is: extracting data from the data sequence output by the sensor according to different time periods, and fusing the data by utilizing an EKF algorithm; and then, carrying out secondary fusion on output results of the EKF algorithms of various periods, namely screening or averaging data at the current moment at common multiple time points of the periods of different EKF algorithms so as to reduce the influence of abnormal interference data of the sensor on the measurement result.
In a preferred embodiment of the present invention, the integrated navigation method of the present invention can be applied to an integrated navigation system of an unmanned boat.
FIG. 2 is a block diagram of a combined navigation system according to an embodiment of the present invention. As shown in FIG. 2, in the system, users can adopt GPS satellite compassThe basic thought of the combined navigation by the compass, the electronic compass and the inertial navigation unit (IMU) is as follows: the method is characterized in that inertial navigation (IMU) is used as main navigation equipment, a GPS satellite compass and an electronic compass are used as auxiliary navigation equipment, the GPS satellite compass provides position and speed information of an unmanned boat, the electronic compass provides heading angle information of the unmanned boat, and the inertial navigation (IMU) provides position, speed and attitude information of the unmanned boat. The state quantities of the EKF are selected as follows (ignoring the unmanned boat altitude h and the natural speed v U Variations of (d):
wherein, the first and the second end of the pipe are connected with each other,
respectively representing three attitude angle errors; delta v E 、δv N Representing east and north velocity errors, respectively; δ L, δ λ represent longitude error and latitude error, respectively; epsilon bx 、ε by 、ε bz Respectively representing constant drift of the three-axis gyroscope;
representing the zero offset error of the accelerometer (east and north), respectively.
The observed mass of EKF was selected as follows:
wherein psi i -ψ g Representing the difference value between the heading angle calculated by inertial navigation solution and the filtered heading angle; v. of iE -v gE 、v iN -v gN 、L i -L g 、λ i -λ g Respectively representing east-direction speed, north-direction speed, longitude, latitude and GPS calculated by inertial navigationThe difference of the corresponding data provided by the satellite compass.
The multi-cycle quadratic fusion EKF algorithm takes 4 update cycles (i.e., T =1,2,3,5).
MATLAB/Simulink simulation is carried out on the combined navigation method, and the effect of the combined navigation method provided by the invention is verified. Fig. 3 is a Simulink simulation structure diagram according to an embodiment of the present invention, wherein the original navigation data generator is used for simulating a motion state of the unmanned vehicle under a given condition, so as to generate error-free GPS satellite compass data, electronic compass data, and inertial navigation data. Each sensor simulation module (GPS satellite compass simulation, electronic compass simulation, gyroscope simulation, accelerometer simulation, magnetometer simulation) is used to superimpose error noise on the corresponding error-free data, and the error characteristics of each sensor in the simulation are set as shown in the following table.
The experimental verification effect of the embodiment of the present invention is explained below.
As can be seen from the simulation results of fig. 4 to fig. 7, the position, velocity, and attitude angle errors output by the inertial navigation system diverge with time, and the position errors output by the GPS satellite compass are not accumulated with time, but are difficult to meet the requirement of accurate navigation due to low accuracy. After the data fusion is carried out by adopting the combined navigation algorithm, the position error is obviously reduced, the output is stable, and the divergence trend is avoided. Compared with the traditional EKF-based integrated navigation method, the integrated navigation method based on the multi-period secondary fusion EKF has better inhibition effect on error fluctuation, the output data error is smaller and more stable, and the fusion data output at the position with larger data fluctuation can better approach the true value.
And evaluating the performances of the two combined navigation algorithms by adopting a Root Mean Square Error (RMSE), wherein the RMSE reflects the deviation between the navigation data output by the combined navigation algorithm and a true value, and the smaller the deviation is, the closer the combined navigation data is to the true value is. The root mean square error is defined as follows:
wherein RMSE is the root mean square error, n is the number of observations, X obs,i Is the i-th observed value, X real,i The true value at the ith time.
The root mean square error statistics for the two combined navigation algorithms are shown in the following table:
as can be seen from the above table data, compared with the conventional EKF-based integrated navigation method, the EKF-based integrated navigation method based on multi-cycle secondary fusion provided by the present invention can provide more accurate navigation data.
Finally, those skilled in the art will appreciate that the systems and methods described herein are not inherently related to any particular apparatus and may be implemented by any suitable combination of components. In addition, various types of general purpose devices may be used with the teachings described herein, and special purpose devices may be constructed to perform the method steps described herein. The present embodiments are also described herein, which are intended in all respects to be illustrative rather than restrictive. Those skilled in the art will appreciate that many different combinations of hardware, software, and firmware will be suitable for practicing the present invention. For example, the software may be implemented in a variety of different programming or description languages, such as Assembler, C/C + +, perl, shell, PHP, java, and so forth.
Moreover, other implementations of the invention will be apparent to those skilled in the art from consideration of the specification of the invention disclosed herein. The various aspects of the embodiments may be used alone or in any combination in the systems and methods of the present invention. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the invention being indicated by the following claims.
Claims (1)
1. An integrated navigation method based on a multicycle secondary fusion EKF algorithm is characterized by comprising the following steps:
step 100, setting a data updating period T = T, and performing data prediction and updating at each time point by adopting an EKF algorithm to obtain an estimation of a state parameter; the data updating period t is adjusted according to the actual performance and the data updating rate of the integrated navigation system and is used as the data updating of the main thread;
step 200, setting a data updating period T =2t,3t,5t ⋯, extracting partial data from the output data of the actual sensors respectively, and obtaining multiple estimates of state parameters by adopting an EKF algorithm; the data updating period is combined by taking any multiple of t, and can be adjusted according to the performance of an actual sensor and a computer;
step 300, performing secondary fusion on the estimation results in different cycle states at the time of common multiple of all data updating cycles, namely screening or averaging the estimation results obtained in all cycles, and taking the obtained value as an effective estimation value of the current time of all cycles;
screening or averaging the estimation results in different periodic states, specifically, processing the estimation results in different periodic states by adopting an averaging or weighted average method;
the navigation result finally output by the navigation system is taken from the updating result with the data updating period being t, and the updating results of other periods 2t,3t and 5t ⋯ are used for providing secondary fusion data so as to reduce the interference of abnormal data.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110918040.4A CN113639754B (en) | 2021-08-11 | 2021-08-11 | Combined navigation method based on multi-period secondary fusion EKF algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110918040.4A CN113639754B (en) | 2021-08-11 | 2021-08-11 | Combined navigation method based on multi-period secondary fusion EKF algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113639754A CN113639754A (en) | 2021-11-12 |
| CN113639754B true CN113639754B (en) | 2023-02-07 |
Family
ID=78420742
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110918040.4A Active CN113639754B (en) | 2021-08-11 | 2021-08-11 | Combined navigation method based on multi-period secondary fusion EKF algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113639754B (en) |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112556690A (en) * | 2020-11-09 | 2021-03-26 | 北京全路通信信号研究设计院集团有限公司 | Multi-source sensor fusion positioning method and device |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7289906B2 (en) * | 2004-04-05 | 2007-10-30 | Oregon Health & Science University | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
| CN103414450B (en) * | 2013-08-01 | 2016-02-03 | 河南大学 | The real-time multi tate H ∞ fused filtering method of noise statistics unknown system |
| CN103455675B (en) * | 2013-09-04 | 2016-08-24 | 哈尔滨工程大学 | A kind of non-linear asynchronous multiple sensors information fusion method based on CKF |
| CN103743401A (en) * | 2013-12-31 | 2014-04-23 | 电子科技大学 | Asynchronous fusion method based on multi-model flight path quality |
| CN110926466A (en) * | 2019-12-14 | 2020-03-27 | 大连海事大学 | Multi-scale data blocking algorithm for unmanned ship combined navigation information fusion |
-
2021
- 2021-08-11 CN CN202110918040.4A patent/CN113639754B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112556690A (en) * | 2020-11-09 | 2021-03-26 | 北京全路通信信号研究设计院集团有限公司 | Multi-source sensor fusion positioning method and device |
Non-Patent Citations (1)
| Title |
|---|
| 基于有理数倍采样的异步数据融合算法研究;葛泉波等;《电子学报》;20060325(第03期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113639754A (en) | 2021-11-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112013836B (en) | Attitude heading reference system algorithm based on improved adaptive Kalman filtering | |
| CN107525503B (en) | Adaptive cascade Kalman filtering method based on combination of dual-antenna GPS and MIMU | |
| CN106990426B (en) | Navigation method and navigation device | |
| CN111024064B (en) | SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering | |
| EP1642089B1 (en) | Method and system for improving accuracy of inertial navigation measurements using measured and stored gravity gradients | |
| Jung et al. | Inertial attitude and position reference system development for a small UAV | |
| CN109931955B (en) | Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering | |
| CN109945859B (en) | Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering | |
| Oh | Multisensor fusion for autonomous UAV navigation based on the Unscented Kalman Filter with Sequential Measurement Updates | |
| CN108761512A (en) | A kind of adaptive CKF filtering methods of missile-borne BDS/SINS deep combinations | |
| CN110017837A (en) | A kind of Combinated navigation method of the diamagnetic interference of posture | |
| Khalaf et al. | Novel adaptive UKF for tightly-coupled INS/GPS integration with experimental validation on an UAV | |
| CN110849360A (en) | Distributed relative navigation method for multi-machine cooperative formation flight | |
| CN111190207B (en) | PSTCSDREF algorithm-based unmanned aerial vehicle INS BDS integrated navigation method | |
| Sokolović et al. | INS/GPS navigation system based on MEMS technologies | |
| CN116007620A (en) | Combined navigation filtering method, system, electronic equipment and storage medium | |
| CN102506875A (en) | Method and device for navigating unmanned aerial vehicle | |
| Zorina et al. | Enhancement of INS/GNSS integration capabilities for aviation-related applications | |
| CN112284388B (en) | Unmanned aerial vehicle multisource information fusion navigation method | |
| CN113639754B (en) | Combined navigation method based on multi-period secondary fusion EKF algorithm | |
| CN110873577B (en) | Underwater rapid-acting base alignment method and device | |
| Kortunov et al. | Integrated mini INS based on MEMS sensors for UAV control | |
| Yang et al. | A novel orientation recursive algorithm aiming at catastrophe signals in MWD | |
| Eldesoky et al. | Improved position estimation of real time integrated low-cost navigation system using unscented kalman filter | |
| Bayat et al. | An augmented strapdown inertial navigation system using jerk and jounce of motion for a flying robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB03 | Change of inventor or designer information | ||
| CB03 | Change of inventor or designer information |
Inventor after: Nie Yong Inventor after: Luo Zhenxiong Inventor after: Lv Xiaowen Inventor after: Tang Jianzhong Inventor after: Li Zhenhui Inventor after: Sun Xiangwei Inventor after: Chen Zheng Inventor before: Luo Zhenxiong Inventor before: Lv Xiaowen Inventor before: Tang Jianzhong Inventor before: Nie Yong Inventor before: Li Zhenhui Inventor before: Sun Xiangwei Inventor before: Chen Zheng |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |