CN111964667B - geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm - Google Patents

geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm Download PDF

Info

Publication number
CN111964667B
CN111964667B CN202010635866.5A CN202010635866A CN111964667B CN 111964667 B CN111964667 B CN 111964667B CN 202010635866 A CN202010635866 A CN 202010635866A CN 111964667 B CN111964667 B CN 111964667B
Authority
CN
China
Prior art keywords
geomagnetic
positioning
matching
navigation
domain
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
Application number
CN202010635866.5A
Other languages
Chinese (zh)
Other versions
CN111964667A (en
Inventor
张东丽
尚俊娜
孙建强
刘新华
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Dianzi University
Original Assignee
Hangzhou Dianzi University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hangzhou Dianzi University filed Critical Hangzhou Dianzi University
Priority to CN202010635866.5A priority Critical patent/CN111964667B/en
Publication of CN111964667A publication Critical patent/CN111964667A/en
Application granted granted Critical
Publication of CN111964667B publication Critical patent/CN111964667B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a geomagnetism-INS integrated navigation method based on a particle filter algorithm, which comprises the following steps: step S2: establishing a discrete geomagnetic database according to the acquired geomagnetic information and acquiring a motion trail on a geomagnetic chart; step S3: performing trend matching by using a derivative dynamic time warping algorithm to obtain a positioning result; step S4: performing auxiliary geomagnetic matching positioning calculation through the motion track calculated by inertial navigation to obtain another positioning result; step S5: fusing the positioning result of the step S3 and the positioning result of the step S4 by adopting a particle filter algorithm to obtain the final positioning estimation of the geomagnetic navigation; the method adopts a derivative dynamic time warping method to carry out trend matching, and compared with a point-to-point matching algorithm, the method has the advantages of small error and higher accuracy; the motion trail of inertial navigation solution is used for assisting geomagnetic matching positioning solution, and the positioning results of the inertial navigation solution and the geomagnetic matching positioning solution are fused by a particle filtering method, so that the positioning estimation is more accurate.

Description

geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm
Technical Field
The invention relates to the technical field of geomagnetic navigation, in particular to a geomagnetic-INS combined navigation method based on a particle filter algorithm.
Background
With the continuous development of the internet of things in the 21 st century and the popularization of intelligent terminals, the demand of users for Location Based Services (LBS) continues to increase. Users are eager to obtain a location information service with higher location accuracy in various geographical locations. Currently, there are various navigation methods available. Satellite navigation systems are the more popular and very important approach, such as the beidou navigation system. The GPS navigation system, the GLONASS navigation system and other satellite navigation systems can provide navigation and positioning services all the day, thereby providing high-precision position information for users. Due to the global popularity of smart phones and the internet, satellite navigation and positioning services have profoundly influenced people's daily lives, as well as social and economic development. However, in some special environments, such as deep valleys, areas with urban high-rise building concentration, department stores, closed stadiums, conduit corridors and underground garages, when the satellite signals are blocked, the satellite positioning may be more wrong or even impossible, or in the event of signal interference and war and other adverse conditions, the satellite may not provide normal positioning service and may output wrong positioning information. However, in the background environment in which the satellite signal is affected by occlusion, it is usually an area where the user wants to locate and obtain a location service. In order to consider the needs of national defense safety and some civil fields, research and exploration of novel navigation positioning technology become hot spots of research in the navigation positioning field.
Geomagnetic navigation is one of the hot techniques of positioning research at present, and utilizes the characteristics of a geomagnetic field, which is a vector field and is a function of a spatial position. Currently, global and local mathematical models and databases of geomagnetic fields are becoming more and more sophisticated. High sensitivity, high reliability, small size and low cost geomagnetic sensors have been developed in succession. Extended Kalman Filters (EKFs) and traceless kalman filters (UKFs), particle filtering and other filtering techniques, and geomagnetic matching algorithms are becoming mature, so you can use the mapping relationship between unique magnetic field vectors at arbitrary positions in the near-earth space to realize geomagnetic navigation. The geomagnetic navigation technology does not need a large amount of equipment cost, does not have radiation, is low in cost and strong in concealment, and errors cannot accumulate along with time. It has been increasingly used in the marine, land, air and sky fields.
The geomagnetic matching in the geomagnetic navigation is easy to implement compared with geomagnetic filtering, and the geomagnetic navigation has a good application prospect in various navigation fields, generally, the geomagnetic navigation is not used as a single navigation technology, most of the geomagnetic navigation is used for assisting an inertial device to perform navigation, and the inertial device provides mileage information and a movement direction for the geomagnetic navigation, so the geomagnetic navigation is often used together with the inertial navigation. The combined positioning accuracy of geomagnetic navigation and inertial device is much higher than that of pure inertial navigation, which is beneficial to the popularization of navigation systems.
In the current geomagnetic matching navigation, the geomagnetic-assisted inertial integrated navigation has the advantages of complete independence, all weather, whole area, passivity, no radiation and the like, and is a novel integrated navigation method with great potential. The method has wide application prospect in the fields of external positioning and aviation navigation, and the application of geomagnetic-assisted inertial integrated navigation breaks through the key technology of filtering of an integrated navigation system.
The classical Kalman filtering is established on the basis of two important assumptions, wherein firstly, a state equation of a system can be completely expressed by using a linear equation, secondly, noise conforms to Gaussian noise distribution, the problem that the state equation is not easy to meet in engineering is solved, firstly, the establishment of a completely prepared state model is difficult, particularly, when the sampling frequency of a sensor is low, the error of the model is large, and the noise of the sensor does not strictly conform to the Gaussian noise distribution actually. In this case, the denoising effect of the classical kalman filter is very limited. Although the extended kalman filter is improved to a great extent compared with the classical kalman filter when dealing with the non-linearity problem, neglecting higher-order terms of more than two orders will affect the result, and meanwhile, the calculation of the jacobian matrix will consume a lot of time, especially in the case of higher dimensionality, the amount of data calculation will be larger. This is obviously a very negative effect on the navigation filtering, and after all, the real-time nature of the navigation is also a very important requirement. In order to obtain better nonlinear processing capability and avoid the calculation of a Jacobian matrix, an unscented Kalman filtering method is provided. Unscented Kalman Filter (UKF), a nonlinear filtering method based on Unscented Transformation (UT) Transformation.
In the above filtering methods, it is necessary to assume that the system noise and the measurement noise are gaussian noise, and different forms of linearization are performed on the linear system, which is not easy to implement in practice.
For example, "a navigation method and apparatus based on extended kalman filter" disclosed in chinese patent literature, the publication number of which is: CN102519463A, filing date thereof: in 12/13/2011, the method performs navigation positioning through an extended Kalman filtering algorithm, although the extended Kalman filtering is improved to a great extent compared with the classical Kalman filtering when a nonlinear problem is processed, the result is influenced by neglecting a high-order term above the second order, meanwhile, the calculation of the Jacobian matrix consumes a large amount of time, and particularly under the condition of high dimensionality, the calculation amount of data is larger.
Disclosure of Invention
The invention mainly solves the problems of large error and inaccurate positioning of geomagnetic navigation in the prior art; the geomagnetic-INS integrated navigation method based on the particle filter algorithm is provided, and the positioning error is corrected by adopting the filter technology to obtain more accurate positioning estimation.
The technical problem of the invention is mainly solved by the following technical scheme: a geomagnetic-INS integrated navigation method based on a particle filter algorithm comprises the following steps:
step S1: acquiring geomagnetic information through a triaxial geomagnetic sensor module and an inertial device module;
step S2: establishing a discrete geomagnetic database according to the acquired geomagnetic information and acquiring a motion track on a geomagnetic map;
step S3: performing trend matching by using a derivative dynamic time warping algorithm to obtain a positioning result;
step S4: performing assisted geomagnetic matching positioning calculation through a motion trajectory calculated by Inertial Navigation (INS) to obtain another positioning result;
step S5: and fusing the positioning result of the step S3 and the positioning result of the step S4 by adopting a particle filter algorithm to obtain the final positioning estimation of the geomagnetic navigation.
The three-axis geomagnetic sensor module is used for selecting a to-be-navigated area in the process of establishing a discrete database because the geomagnetic field intensity measured by the three-axis geomagnetic sensor module is vector intensity, carrying out grid division on the area, paying attention to the fact that the directions of measuring points on a map are consistent, ensuring the uniqueness of geomagnetic information in space, and guaranteeing geomagnetic information sequence matching in the online matching process; geomagnetic database acquisition conditions: when geomagnetic data is collected, it is ensured that the interval between the measurement point and the measurement point is consistent, and the interval is within a certain range, and the interval should not be too large, otherwise, the accuracy of the geomagnetic database is affected. Meanwhile, when building a library, it is necessary to ensure that the measurement directions of the triaxial geomagnetic sensor modules are consistent. In the process of collecting geomagnetic data, collecting a group of data at measuring points, removing total errors by using the Lauda criterion, averaging the values of the measuring points to obtain data of one measuring point, and then optimally segmenting and subdividing data points in a grid by using a generalized continuation approximation method to establish a high-precision geomagnetic database.
Preferably, in step S3, the specific method for performing trend matching by using the derivative dynamic time warping algorithm includes: in geomagnetic matching, the related information of the shape is obtained by considering the first derivative of the sequence, and the morphological feature of the sequence is taken into account, and the Derivative Dynamic Time Warping (DDTW) is implemented by constructing an n × m matrix, wherein (i) in the matrix isth,jth) The parameter comprises points p on two time seriesiAnd q isjD forx[p]And Dx[q]The square of the difference between the derivatives, which is different from DTW, is usually calculated by the following equation:
the local distance based on derivative estimation is calculated as:
d(i,j)＝(D(pi)-D(qj))2
Figure GDA0003505176360000031
derived from the above equation, the derivative estimate Dx[p]Through the point piAnd the DDTW estimates a second node and a penultimate node of the time sequence according to the slope of the straight line of the left adjacent point of the point and the average value of the slopes of the straight lines of the left adjacent point and the right adjacent point of the point, the geomagnetic sequence to be matched is smoothed before geomagnetic matching, a generalized continuation fitting method is adopted, and the time complexity of the DDTW is O (mn).
Preferably, in step S4, the specific method for performing the assisted geomagnetic matching positioning calculation through the motion trajectory calculated by the INS is as follows: integrating geomagnetic sequences by adopting a sliding window form, wherein the length of the sliding window is mainly obtained by empirical values, reducing the range of the geomagnetic field strong sequences to be matched in a discrete geomagnetic database by mileage information resolved by a round number counter, positioning and resolving by adopting a sliding matching mode, so that the matching similarity between sequences is represented, a threshold value of the matching similarity is represented, the matching similarity between sequences is mainly obtained by the empirical values of the matching similarity between sequences, when a matching condition is satisfied, a matched positioning position is output, otherwise, a window of the geomagnetic database field strength sequences slides leftwards or rightwards.
Preferably, in step S5, when performing final localization estimation by using a particle filter algorithm, the method includes the following steps:
step S51: initialization, extracting random samples according to prior conditional probability
Figure GDA0003505176360000041
Wherein N is the number of samples;
step S52: prediction process, from state equations and random samples of the system
Figure GDA0003505176360000042
Obtaining further predicted samples
Figure GDA0003505176360000043
Step S53: updating procedure from the measured value z at the time kkAnd predicted value z 'of the measured value'kCalculating the weight of each particle, and normalizing the weight to obtain w, wherein i is 1,2 … N;
step S54: preliminary state estimation, calculating an estimate of the state from the particles and their weights
Figure GDA0003505176360000044
Resampling is performed by using the particles at the time and their weights, so as to obtain a particle sample at the next time, and the process returns to step S52;
step S55: and selecting corresponding particles according to the indexes obtained by resampling, wherein the reconstructed set is a filtering state set, and the average value of the state set is calculated to obtain the final target state.
Preferably, the derivative dynamic time warping algorithm is a trend matching algorithm, and the DDTW does not estimate the first data node and the last data node of the time series, but estimates the data nodes using the second node and the penultimate node.
Preferably, the motion trajectory of the inertial navigation solution is used for assisting geomagnetic matching positioning solution, a geomagnetic sequence needs to be integrated in a sliding window form, the length of the sliding window is mainly obtained by an empirical value, the range of geomagnetic field intensity sequences to be matched in the discrete geomagnetic database is reduced by mileage information calculated by the wheel number meter, positioning solution is performed in a sliding matching mode, and the method can reduce the calculation complexity in the matching process.
Preferably, the filtering algorithm for fusing the positioning results of the two is mainly to approximate the probability by using a method similar to statistical sampling when facing the problem that the numerical method is not easy to solve, and further calculate to obtain an optimal approximate solution.
Preferably, the method for establishing the discrete geomagnetic database comprises: the method comprises the steps of ensuring that in the process of acquiring geomagnetic data, the intervals between measuring points are kept consistent, the intervals are not too large, otherwise, the precision of a geomagnetic database is influenced, ensuring that the measuring directions of the triaxial geomagnetic sensor modules are kept consistent while the database is established, removing gross errors through a Layouta criterion after the measuring points acquire a group of data, averaging the values of the measuring points to obtain data of one measuring point, and finally performing optimal piecewise approximation on data points in a grid through a generalized continuation approximation method to establish a high-precision segmentation geomagnetic database.
Preferably, the generalized continuation approximation method comprises: let Ψ be a defined domain, which is divided into n unit sub-domains Ψ that do not overlap with each othere(e 1, 2.., n), then:
Figure GDA0003505176360000051
marking the extended continuation domain as psi'eThen, then
Figure GDA0003505176360000052
And is provided with
Figure GDA0003505176360000053
L of psi'eM nodes therein, of which Ψ iseHas r (r) nodes<m), in continuation domain Ψ'eWhen constructing an approximation function, for making the unit ΨeThe function value on the boundary is consistent with the function value of each adjacent unit, and the unit psieThe m boundary points should satisfy the interpolation limiting factors on the boundary points; finally, only the part of the approximation function obtained in each continuation domain on the unit definition domain is taken and then spliced to form the approximation function of the whole definition domain;
let Ψ 'on continuation domain'eThe approximation function of (a) is:
Figure GDA0003505176360000054
where t is the number of terms of the approximation function, (h)1,h2,…,hp) Is ΨeA set of basis functions; epsiloni(i ═ 1,2,. p) is the undetermined coefficient; to continuation domain Ψ'eIs carried out in the unit domain ΨeThe boundary points of the unit domain are in accordance with the interpolation conditions, and the least square fitting processing is carried out on other points, so that undetermined coefficients of the approximation function in the unit domain can be obtained;
after the approximation function in each unit domain is solved, the approximation functions of the whole definition domain can be obtained by splicing:
Figure GDA0003505176360000055
preferably, the method for acquiring the motion trajectory comprises the following steps: moving a distance on the two-dimensional geomagnetic chart to obtain a geomagnetic data sequence (m)1,m2,…,mk) And measuring a set of accelerations (a) with the inertial device module1,a2,…,ak) And azimuth data series (theta)1,θ2,…,θk) (ii) a Drawing a motion track in a geomagnetic chart through dead reckoning;
the dead reckoning method comprises the following steps: let us assume at tnTime of day, known position
Figure GDA0003505176360000056
And the speed of movement at the current moment
Figure GDA0003505176360000057
And course angle
Figure GDA0003505176360000058
At this time, t is obtained by dead reckoningn+1Time of day position
Figure GDA0003505176360000059
The calculation formula is as follows:
Figure GDA00035051763600000510
Figure DA00035051763631826863
the invention adopts the derivative dynamic time warping method to carry out trend matching, and compared with a point-to-point matching algorithm, the method has the advantages of small error and higher accuracy; (2) the motion trail of inertial navigation solution is used for assisting geomagnetic matching positioning solution, and the positioning results of the inertial navigation solution and the geomagnetic matching positioning solution are fused by a particle filtering method, so that the positioning estimation is more accurate.
Drawings
Fig. 1 is a diagram of the alignment of time series generated by the DDTW algorithm of the first embodiment.
Fig. 2 is a schematic diagram of a sliding matching process of the geomagnetic field sequence obtained by online measurement according to the first embodiment.
Fig. 3 is a schematic flow chart of particle filtering according to the first embodiment.
FIG. 4 is a graph of the EKF _ UKF _ PF filtering versus effect of the first embodiment.
FIG. 5 is an error map of the estimated values of EKF _ UKF _ PF of the first embodiment.
Detailed Description
The technical scheme of the invention is further specifically described by the following embodiments and the accompanying drawings.
The first embodiment is as follows: a geomagnetic-INS combined navigation method based on a particle filtering algorithm, as shown in fig. 1,2 and 3, includes the following steps:
the method comprises the following steps: and a discrete geomagnetic database is established, so that the interval between the measuring point and the measuring point is kept consistent in the process of acquiring geomagnetic data, the interval is not too large, and otherwise the precision of the geomagnetic database is influenced. The method comprises the steps of establishing a database, ensuring that the measuring directions of triaxial geomagnetic sensor modules are kept consistent, removing gross errors by means of Layida criteria after a group of data is collected at a measuring point, averaging the numerical values of the measuring point to obtain data of the measuring point, and finally performing optimal piecewise approximation on data points in a grid by means of generalized continuation approximation to establish a high-precision subdivided geomagnetic database.
The generalized continuation approach is to extend each subunit domain to nearby adjacent units to form a continuation domain. Therefore, the information outside the unit can be fully utilized, the approximation function in the unit can fully absorb the information of the adjacent unit, and the approximation function on the unit domain can be ensured to be coordinated with the approximation function of the adjacent unit.
The principle is as follows: let Ψ be a defined domain, which is divided into n unit sub-domains Ψ that do not overlap with each othere(e 1, 2.., n), then:
Figure GDA0003505176360000062
marking the extended continuation domain as psi'eThen, then
Figure GDA0003505176360000063
And is provided with
Figure GDA0003505176360000064
L of psi'eM nodes therein, of which ΨeHas r nodes (r)<m), in continuation domain Ψ'eWhen constructing an approximation function, for making the unit ΨeThe function value on the boundary is consistent with the function value of each adjacent unit, and the unit psieThe m boundary points of (1) should satisfy the interpolation limiting factor on the boundary points. And finally, only taking the part of the approximation function obtained in each continuation domain on the unit definition domain, and then splicing the approximation functions to form the approximation function of the whole definition domain.
Let Ψ 'on continuation domain'eThe approximation function of (a) is:
Figure GDA0003505176360000071
where t is the number of terms of the approximation function, (h)1,h2,…,hp) Is ΨeA set of basis functions; epsiloniAnd (i ═ 1,2,. p) is a pending coefficient. To continuation domain Ψ'eIs carried out in the unit domain ΨeThe boundary points of the unit domain are in accordance with the interpolation conditions, and the least square fitting processing is carried out on other points, so that the undetermined coefficient of the approximation function in the unit domain can be obtained.
After the approximation function in each unit domain is solved, the approximation functions of the whole definition domain can be obtained by splicing:
Figure GDA0003505176360000072
step two: moving a distance on the two-dimensional geomagnetic chart to obtain a geomagnetic data sequence (m)1,m2,…,mk) And measuring a set of accelerations (a) with the inertial device module1,a2,…,ak) And azimuth data sequence (theta)1,θ2,…,θk). Track the motion in the dead reckoningShown in the geomagnetic chart.
Dead reckoning principle: let us assume at tnTime of day, known position
Figure GDA0003505176360000073
And the moving speed of the current moment
Figure GDA0003505176360000074
And course angle
Figure GDA0003505176360000075
At this time, t is obtained by dead reckoningn+1Time of day position
Figure GDA0003505176360000076
The calculation formula is as follows:
Figure GDA0003505176360000077
and step three, integrating the geomagnetic sequence in the database corresponding to the mileage information on the motion trail with the online measured geomagnetic sequence by using a Derivative Dynamic Time Warping (DDTW) algorithm, as shown in the attached drawing 1. In the geomagnetic matching, the morphological feature of the sequence is taken into account by acquiring the relevant information of the shape by considering the first derivative of the sequence. The Derivative Dynamic Time Warping (DDTW) method is implemented by constructing an n × m matrix, where (i) in the matrixth,jth) The parameter comprises points p on two time seriesiAnd q isjD forx[p]And Dx[q]The square of the difference between the derivatives, which is different from DTW. The local distance based on derivative estimation is calculated as:
d(i,j)＝(D(pi)-D(qj))2
Figure GDA0003505176360000078
derived from the above equation, the derivative estimate Dx[p]Through the point piDDTW estimates the second and penultimate nodes of the time series from the slope of the line to the left neighbor of the point and the average of the slopes of the lines through the left and right neighbors of the point, with a time complexity of o (mn).
In an actual environment, the acquisition amount of geomagnetic data for a long time is large, a DDTW mode is independently adopted to be not beneficial to the effect of real-time positioning, a geomagnetic sequence is integrated in a sliding window mode, the length of the sliding window is mainly obtained by empirical values, the range of geomagnetic field strong sequences to be matched in a discrete geomagnetic database is reduced through mileage information resolved by a wheel number meter, positioning resolving is carried out in a sliding matching mode, and the method can reduce the calculation complexity in the matching process. Dividing the planned positioning area into grids at equal intervals, collecting the geomagnetic characteristic quantity of each grid point, and then dividing the coordinate p of each pointi(xi,yi) Forming a fingerprint vector MP [ m, p ] with the corresponding geomagnetic characteristic quantity]Then, alternately intercepting the sliding window according to the time sequence, setting the size k of the sliding window according to the positioning time and the precision, and establishing a geomagnetic modulus time sequence B based on the size of the sliding windowi＝{MP1,MP2,MP3,…,MPiFig. 2 shows a time series model with a sliding window length of 4, and establishes geomagnetic field modulus value fingerprint sequences a of the same specification for data to be positioned according to the size of the sliding windowi＝{M1,M2,M3,…,MjAnd in the positioning process, matching the geomagnetic time sequence to be matched with each geomagnetic time sequence in the geomagnetic fingerprint database to obtain an optimal matching result.
And step four, filtering the result of geomagnetic matching positioning and the position information positioning result obtained by INS positioning calculation through a particle filtering algorithm, and filtering out positioning errors caused by mismatching to obtain a stable, continuous and high-precision positioning result.
The particle filter algorithm is a filtering method based on Bayes estimation, and the basic idea is to describe probability distribution by searching a group of random samples which are propagated in a state space, the samples become particles, then on the basis of re-measurement, the actual probability distribution is approximated by adjusting the weight of each particle and the position of the sample, and the mean value of the samples is taken as the estimated value of the system, thereby obtaining the state estimation in the sense of minimum variance. Assuming a continuous time-varying nonlinear system can be described as:
Figure GDA0003505176360000081
in the above formula xtIndicating the state quantity at time t, ytRepresents the observed quantity at time t, ft，htRepresenting the state transition function and observation function at time t, yt，ntRepresenting the state noise and observation noise vectors at time t. In the particle filter algorithm, the required prediction vector and state vector are not directly obtained, but the probability distribution is obtained, and the initial probability density function p (x) of the state is assumed0|y0)＝p(x0) As is known, then the predicted amount of state can be expressed in bayesian recursion as:
p(xt|ytx-1)＝∫p(xt|xt-1)p(xt-1|ytx-1)dxt-1
and the observation of the state is updated as:
Figure GDA0003505176360000091
in the above formula:
p(yt|ytx-1)＝∫p(yt|xt)p(xt|ytx-1)dxt
wherein: y istx＝{y1,y2,...,yt}. Obviously, the integral operation in the formula brings inconvenience to the solution, and is not favorable for realizing p (x)0x|y0x) The Monte Carlo method is introduced for this particle filtering, and the integral is converted into a sample weighting value at a finite point, and these samples and the corresponding weighting values are called particles. Suppose that N independently distributed samples are collectedThis, then the posterior probability can be approximated as:
Figure GDA0003505176360000092
since the objective density function p (x) is complex in practice, it is difficult to obtain the weight of the particles, and therefore, the importance density function is often used for sampling and expressed as q (x)0x|ytx) And q (x)0x|ytx) Need to contain p (x)0x|ytx) The method for solving the importance density function of all the supporting point sets comprises the following steps:
Figure GDA0003505176360000093
where w represents a weight corresponding to a sample, and its expression is:
Figure GDA0003505176360000094
simultaneously defining:
Figure GDA0003505176360000095
Figure GDA0003505176360000096
here, normalized weights are expressed and satisfy:
Figure GDA0003505176360000097
and after posterior probability density recursion and sample weight recursion, obtaining:
Figure GDA0003505176360000098
the estimated value of the state is thus:
Figure GDA0003505176360000099
it can be seen from the derivation process above that, it is necessary to first obtain particles from the importance distribution, and then determine a weight updating mode, obtain a state estimation value by combining observation vector recursion, and sequentially recur to obtain a predicted value of the required state quantity on the basis of credible tracking accuracy, and the calculation idea is: and selecting a particle set according to actual requirements, setting an initial value of the particle set, calculating an importance weight and normalizing the weight, then resampling and redistributing the weight, and gradually iterating to output the state estimator.
According to the basic solution idea of particle filtering, the method is a random sampling method, but because the particle filtering has a degradation phenomenon, simple repeated sampling cannot be carried out. To solve this problem, the sampling plan cannot be fixed, and resampling needs to be performed according to the last sampling particle and the corresponding weight.
Firstly, initialization, extracting random samples according to prior conditional probability
Figure GDA0003505176360000101
Wherein N is the number of samples; and step two, prediction process. From the state equations and random samples of the system
Figure GDA0003505176360000102
Obtaining further predicted samples
Figure GDA0003505176360000103
The third step, the updating process, is to calculate the measured value z from the time kkAnd predicted value z 'of the measured value'kCalculating the weight of each particle, and normalizing the weight to obtain w, wherein i is 1,2 … N; and fourthly, estimating the state. Calculating an estimate of a state from particles and their weights
Figure GDA0003505176360000104
And using the timeThe particles and their weights are resampled to get a sample of the particles at the next time instant, and the second step is returned.
The filtering model is established as follows, wherein the system state equation is as follows:
Figure GDA0003505176360000105
the system state equation is further simplified as:
Figure GDA0003505176360000106
the system's observation equation can be expressed as:
Figure GDA0003505176360000107
in the above model, Wk，VkThe white Gaussian noise representing the system state equation and the observation equation, respectively, and the covariance of the system state noise and the observation noise can be expressed as QkAnd Rk。(xk,yk) For true motion trajectory,/k、θkDistance and Direction Angle of motion, (x'k,y'k) The geomagnetic positioning result at the time k is obtained.
The observation matrix is:
Figure GDA0003505176360000111
calculating the importance weight of the particle according to the relation between the predicted quantity and the predicted quantity of the particle and carrying out normalization treatment:
Figure GDA0003505176360000112
Figure GDA0003505176360000113
copying and eliminating the particle set according to the weight, wherein the number of the copied particles of the particle i is deltaiResetting the weight
Figure GDA0003505176360000114
Selecting corresponding particles according to the indexes obtained by resampling, wherein the reconstructed set is a filtering state set, and averaging the state set to obtain a final target state:
Figure GDA0003505176360000115
and filtering by using the result of the geomagnetic matching positioning and the position information positioning result obtained by INS positioning calculation through a particle filtering algorithm, and filtering out the positioning error caused by mismatching to obtain a stable, continuous and high-precision positioning result.
Because each navigation sensor is a sensitive device working under a specially rated physical principle, and advanced navigation instruments are almost all electronic devices, the gas signal acquisition, detection, processing and transmission processes are interfered by peripheral equipment or external environmental influences, the measurement results of each navigation sensor contain noise, and the measured data also deviate from real data. Therefore, the filtering algorithm is an effective method for reducing various error influences, so that the value of the measuring end is closer to the real state. The filtering method widely used is kalman filtering, but the framework of kalman filtering is only suitable for solving the problem of linear filtering, and the problem encountered in reality is generally nonlinear, and in order to solve the problem, the concept of Extended Kalman Filtering (EKF) is introduced. The basis of this idea is to use the principle of taylor expansion on the filtering method.
The classical Kalman filtering is established on the basis of two important assumptions, wherein firstly, a state equation of a system can be completely expressed by using a linear equation, secondly, noise conforms to Gaussian noise distribution, the problem that the state equation is not easy to meet in engineering is solved, firstly, the establishment of a completely prepared state model is difficult, particularly, when the sampling frequency of a sensor is low, the error of the model is large, and the noise of the sensor does not strictly conform to the Gaussian noise distribution actually. In this case, the denoising effect of the classical kalman filter is very limited. Although the extended kalman filter is improved to a great extent compared with the classical kalman filter when dealing with the non-linearity problem, neglecting higher-order terms of more than two orders will affect the result, and meanwhile, the calculation of the jacobian matrix will consume a lot of time, especially in the case of higher dimensionality, the amount of data calculation will be larger. This is obviously a very negative effect on the navigation filtering, and after all, the real-time nature of the navigation is also a very important requirement. In order to obtain better nonlinear processing capability and avoid the calculation of a Jacobian matrix, an unscented Kalman filtering method is provided. Unscented Kalman Filter (UKF), a nonlinear filtering method based on Unscented Transformation (UT) Transformation.
In the above filtering methods, it is necessary to assume that the system noise and the measurement noise are gaussian noise, and different forms of linearization are performed on the linear system, which is not easy to implement in practice. One of the effective methods for solving the problem is a particle filtering method established on the Monte Carlo simulation idea, the particle filtering algorithm is a state estimation method fundamentally different from Kalman filtering, and is a sequential Monte Carlo simulation method established on the basis of Bayes estimation theory, which is proposed only in recent years, the core of the method is that the posterior probability density of system random variables is expressed by using random samples (for the random samples, the random samples can also be called as particles) and corresponding weights thereof, so that the optimal solution based on a state model can be obtained, and the particle filtering method is more suitable for the occasions of non-Gaussian noise nonlinear systems because the model approximation principle in the traditional filtering method is abandoned.
A simple comparison is performed on the three filtering methods, as shown in fig. 4 and 5, the error of the estimated values of the three filters is compared with the error of the estimated values of the real value and the estimated value in the filtering process of the three filtering methods, and it can be seen that the Particle Filtering (PF) has a smaller estimation error than the Extended Kalman Filtering (EKF) and the Unscented Kalman Filtering (UKF), and the filtering result is smoother.
The above-described embodiments are only preferred embodiments of the present invention, and are not intended to limit the present invention in any way, and other variations and modifications may be made without departing from the spirit of the invention as set forth in the claims.

Claims (9)

1. A geomagnetism _ INS combined navigation method based on a particle filter algorithm is characterized by comprising the following steps:
step S1: collecting geomagnetic information through a triaxial geomagnetic sensor module and an inertial device module;
step S2: establishing a discrete geomagnetic database according to the acquired geomagnetic information and acquiring a motion track on a geomagnetic map;
step S3: performing trend matching by using a derivative dynamic time warping algorithm to obtain a positioning result;
step S4: performing assisted geomagnetic matching positioning calculation through a motion trajectory calculated by Inertial Navigation (INS) to obtain another positioning result;
step S5: fusing the positioning result of the step S3 and the positioning result of the step S4 by adopting a particle filter algorithm to obtain the final positioning estimation of the geomagnetic navigation;
in step S4, the specific method for performing assisted geomagnetic matching positioning calculation through the motion trajectory calculated by the INS includes: integrating geomagnetic sequences by adopting a sliding window form, wherein the length of the sliding window is mainly obtained by empirical values, reducing the range of the geomagnetic field strong sequences to be matched in the discrete geomagnetic database by using mileage information resolved by a wheel number counter, performing positioning resolution by adopting a sliding matching mode, outputting the matched positioning position when matching conditions are met, otherwise, sliding the window of the geomagnetic database field strength sequences leftwards or rightwards.
2. The combined geomagnetic-INS navigation method based on particle filter algorithm according to claim 1, wherein in the step S3, the specific method for performing trend matching by using the derivative dynamic time warping algorithm comprises: in geomagnetic matching, the related information of the shape is obtained by considering the first derivative of the sequence, and the morphological feature of the sequence is taken into account, and the Derivative Dynamic Time Warping (DDTW) is implemented by constructing an n × m matrix, wherein (i) in the matrix isth,jth) The parameter comprises points p on two time seriesiAnd q isjD forx[p]And Dx[q]The square of the difference between the derivatives, which is different from DTW, is calculated by the following equation:
the local distance based on derivative estimation is calculated as:
d(i,j)＝(D(pi)-D(qj))2
Figure FDA0003500137030000011
derived from the above equation, the derivative estimate Dx[p]Through the point piAnd the DDTW estimates a second node and a penultimate node of the time sequence according to the slope of the straight line of the left adjacent point of the point and the average value of the slopes of the straight lines of the left adjacent point and the right adjacent point of the point, the geomagnetic sequence to be matched is smoothed before geomagnetic matching, a generalized continuation fitting method is adopted, and the time complexity of the DDTW is O (mn).
3. The combined geomagnetic-INS navigation method based on particle filter algorithm according to claim 1 or 2, wherein in the step S5, when performing geomagnetic navigation final positioning estimation by using the particle filter algorithm, the method comprises the following steps:
step S51: initialization, extracting random samples according to prior conditional probability
Figure FDA0003500137030000012
Wherein N is the number of samples;
step S52: prediction process, from state equations and random samples of the system
Figure FDA0003500137030000021
Obtaining further predicted samples
Figure FDA0003500137030000022
Step S53: updating procedure from the measured value z at the time kkAnd predicted value z 'of the measured value'kCalculating the weight of each particle, and normalizing the weight to obtain w, wherein i is 1,2 … N;
step S54: preliminary state estimation, calculating an estimate of the state from the particles and their weights
Figure FDA0003500137030000023
Resampling is performed by using the particles at the time and their weights, so as to obtain a particle sample at the next time, and the process returns to step S52;
step S55: and selecting corresponding particles according to the indexes obtained by resampling, wherein the reconstructed set is a filtering state set, and the average value of the state set is calculated to obtain the final target state.
4. The method of claim 1, wherein the derivative dynamic time warping algorithm is a trend matching algorithm, and the DDTW does not estimate the first data node and the last data node of the time series, and estimates the data nodes using the second node and the penultimate node.
5. The integrated geomagnetic-INS navigation method based on the particle filter algorithm according to claim 1, wherein the inertial navigation solution is performed with a motion trajectory that assists in geomagnetic matching positioning solution, a sliding window is required to be used for integrating geomagnetic sequences, the length of the sliding window is mainly obtained from empirical values, the range of geomagnetic field intensity sequences to be matched in the discrete geomagnetic database is narrowed through mileage information calculated by a round-robin algorithm, positioning solution is performed in a sliding matching manner, and the method can reduce the calculation complexity in the matching process.
6. The method as claimed in claim 1, wherein the filtering algorithm for fusing the positioning results of the two is mainly used to approximate the probability by using a statistical sampling-like method when the problem that the numerical method is not easy to solve, and further calculate to obtain an optimal approximate solution.
7. The integrated geomagnetic-INS navigation method based on particle filter algorithm according to claim 1, wherein the discrete geomagnetic database is established by: the method comprises the steps of ensuring that in the process of acquiring geomagnetic data, the intervals between measuring points are kept consistent, the intervals are not too large, otherwise, the precision of a geomagnetic database is influenced, ensuring that the measuring directions of the triaxial geomagnetic sensor modules are kept consistent while the database is established, removing gross errors through a Layouta criterion after the measuring points acquire a group of data, averaging the values of the measuring points to obtain data of one measuring point, and finally performing optimal piecewise approximation on data points in a grid through a generalized continuation approximation method to establish a high-precision segmentation geomagnetic database.
8. The integrated geomagnetic-INS navigation method based on particle filter algorithm according to claim 7, wherein the generalized continuation approximation method is: let Ψ be a defined domain, which is divided into n unit sub-domains Ψ that do not overlap with each othere(e 1, 2.., n), then:
Figure FDA0003500137030000031
marking the extended continuation domain as psi'eThen, then
Figure FDA00035001370300000310
And is provided with
Figure FDA0003500137030000032
L of psi'eM nodes therein, of which Ψ iseHas r (r) nodes<m), in continuation domain Ψ'eWhen constructing an approximation function, for making the unit ΨeThe function value on the boundary is consistent with the function value of each adjacent unit, and the unit psieThe m boundary points of (1) should meet the interpolation limiting factor on the boundary points; finally, only taking the part of the approximation function obtained in each continuation domain on the unit definition domain, and then splicing the approximation functions to form the approximation function of the whole definition domain;
let Ψ 'on continuation domain'eThe approximation function of (a) is:
Figure FDA0003500137030000033
wherein t is the number of terms of the approximation function, (h)1,h2,…,hp) Is ΨeA set of basis functions; epsiloni(i ═ 1,2,. p) is the undetermined coefficient; to continuation domain Ψ'eIs carried out in the unit domain ΨeThe boundary points of the unit domain are in accordance with the interpolation conditions, and the least square fitting processing is carried out on other points, so that undetermined coefficients of the approximation function in the unit domain can be obtained;
after the approximation function in each unit domain is solved, the approximation functions of the whole definition domain can be obtained by splicing:
Figure FDA0003500137030000034
9. ground based on particle filter algorithm according to claim 1The magnetic _ INS integrated navigation method is characterized in that the motion trail is obtained by the following steps: moving a distance on the two-dimensional geomagnetic chart to obtain a geomagnetic data sequence (m)1,m2,…,mk) And measuring a set of accelerations (a) with the inertial device module1,a2,…,ak) And azimuth data sequence (theta)1,θ2,…,θk) (ii) a Drawing a motion track in a geomagnetic chart through dead reckoning;
the dead reckoning method comprises the following steps: let us assume at tnTime of day, known position
Figure FDA0003500137030000035
And the speed of movement at the current moment
Figure FDA0003500137030000036
And course angle
Figure FDA0003500137030000039
At this time, t is obtained by dead reckoningn+1Time of day position
Figure FDA0003500137030000037
The calculation formula is as follows:
Figure FDA0003500137030000038
CN202010635866.5A 2020-07-03 2020-07-03 geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm Active CN111964667B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010635866.5A CN111964667B (en) 2020-07-03 2020-07-03 geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010635866.5A CN111964667B (en) 2020-07-03 2020-07-03 geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm

Publications (2)

Publication Number Publication Date
CN111964667A CN111964667A (en) 2020-11-20
CN111964667B true CN111964667B (en) 2022-05-20

Family

ID=73361059

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010635866.5A Active CN111964667B (en) 2020-07-03 2020-07-03 geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm

Country Status (1)

Country Link
CN (1) CN111964667B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112906782B (en) * 2021-02-07 2024-01-26 江西科技学院 Track static inspection historical data matching method based on DTW and least square estimation
CN113932806B (en) * 2021-10-15 2023-08-25 北京航空航天大学 High-speed aircraft inertia/geomagnetic matching search area self-adaptive combined navigation method
CN114061591B (en) * 2021-11-18 2022-07-12 东南大学 Contour line matching method based on sliding window data backtracking

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR3023623B1 (en) * 2014-07-09 2023-05-05 Sagem Defense Securite METHOD FOR ESTIMATING THE STATE OF A MOBILE SYSTEM AND CORRESPONDING INERTIAL UNIT.
CN106092093B (en) * 2016-05-26 2019-01-08 浙江工业大学 A kind of indoor orientation method based on earth magnetism fingerprint matching algorithm
CN109883428B (en) * 2019-03-27 2022-11-01 成都电科慧安科技有限公司 High-precision positioning method fusing inertial navigation, geomagnetic and WiFi information
CN110081888B (en) * 2019-05-15 2021-06-22 华南师范大学 Indoor positioning algorithm based on credibility-based inertial navigation and geomagnetic fusion
CN110388926B (en) * 2019-07-12 2021-10-29 杭州电子科技大学 Indoor positioning method based on mobile phone geomagnetism and scene image

Also Published As

Publication number Publication date
CN111964667A (en) 2020-11-20

Similar Documents

Publication Publication Date Title
CN111964667B (en) geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm
CN107255795B (en) Indoor mobile robot positioning method and device based on EKF/EFIR hybrid filtering
Wang et al. A particle filter-based matching algorithm with gravity sample vector for underwater gravity aided navigation
Abtew et al. Spatial Analysis for Monthly Rainfall in South Florida 1
CN109141413B (en) EFIR filtering algorithm and system with data missing UWB pedestrian positioning
CN110057354B (en) Geomagnetic matching navigation method based on declination correction
CN105760811B (en) Global map closed loop matching process and device
CN109186601A (en) A kind of laser SLAM algorithm based on adaptive Unscented kalman filtering
CN105509739A (en) Tightly coupled INS/UWB integrated navigation system and method adopting fixed-interval CRTS smoothing
CN105180938A (en) Particle filter-based gravity sampling vector matching positioning method
CN108521627B (en) Indoor positioning system and method based on WIFI and geomagnetic fusion of HMM
CN106597363A (en) Pedestrian location method in indoor WLAN environment
CN110388926B (en) Indoor positioning method based on mobile phone geomagnetism and scene image
CN110187375A (en) A kind of method and device improving positioning accuracy based on SLAM positioning result
CN105157704A (en) Bayesian estimation-based particle filter gravity-assisted inertial navigation matching method
CN112887899B (en) Positioning system and positioning method based on single base station soft position information
CN109141412B (en) UFIR filtering algorithm and system for data-missing INS/UWB combined pedestrian navigation
CN109655060B (en) INS/UWB integrated navigation algorithm and system based on KF/FIR and LS-SVM fusion
CN109269498B (en) Adaptive pre-estimation EKF filtering algorithm and system for UWB pedestrian navigation with data missing
CN114608568A (en) Multi-sensor-based information instant fusion positioning method
Wendlandt et al. Continuous location and direction estimation with multiple sensors using particle filtering
Zhang et al. A new PHD-SLAM method based on memory attenuation filter
CN111578939A (en) Robot tight combination navigation method and system considering random variation of sampling period
WO2023142205A1 (en) Insar timing phase optimization method and apparatus
CN110927765A (en) Laser radar and satellite navigation fused target online positioning method

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
GR01 Patent grant
GR01 Patent grant