CN108426574A - A kind of MEMS pedestrian navigation methods of the course angle correction algorithm based on ZIHR - Google Patents
A kind of MEMS pedestrian navigation methods of the course angle correction algorithm based on ZIHR Download PDFInfo
- Publication number
- CN108426574A CN108426574A CN201810107852.9A CN201810107852A CN108426574A CN 108426574 A CN108426574 A CN 108426574A CN 201810107852 A CN201810107852 A CN 201810107852A CN 108426574 A CN108426574 A CN 108426574A
- Authority
- CN
- China
- Prior art keywords
- mems
- error
- zero
- zihr
- pedestrian navigation
- 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.)
- Pending
Links
- 238000012937 correction Methods 0.000 title claims abstract description 52
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 21
- 238000000034 method Methods 0.000 title claims abstract description 21
- 238000001914 filtration Methods 0.000 claims abstract description 20
- 238000004364 calculation method Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 22
- 238000005259 measurement Methods 0.000 claims description 20
- 230000008859 change Effects 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 11
- 230000003068 static effect Effects 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 3
- 238000001514 detection method Methods 0.000 abstract description 20
- 230000002401 inhibitory effect Effects 0.000 abstract 1
- 238000012986 modification Methods 0.000 description 6
- 230000004048 modification Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000002474 experimental method Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000005021 gait Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000012612 static experiment Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 239000000654 additive Substances 0.000 description 1
- 230000000996 additive effect Effects 0.000 description 1
- 238000009530 blood pressure measurement Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
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/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
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The MEMS pedestrian navigation methods of the present invention is to provide a kind of course angle correction algorithm based on ZIHR.One：Still time is initially directed at MEMS pedestrian navigation systems using accelerometer and magnetometer；Two：Seek MEMS pedestrian navigation system inertial reference calculation equations and error equation；Three：Zero-speed state-detection is carried out using the output valve of gyroscope and accelerometer；Four：Adjacent moment course angle difference and the relationship at gyroscopic drift and course error angle when solving zero-speed state in ZIHR correction algorithms；Five：Simplified MEMS pedestrian navigation UKF Filtering Models are established, UKF filtering is carried out.The present invention uses the information of Still time to greatest extent, and calculation amount is simultaneously uncomplicated, and can be good at inhibiting course error angular divergence.The zero-speed moment using UKF filters carry out real-time feedback compensation can inhibit well low precision MEMS sensor work long hours navigational parameter error dissipate the problem of, improve the positioning accuracy of pedestrian navigation system.
Description
Technical Field
The invention relates to a pedestrian navigation method, in particular to a pedestrian navigation method based on an inertial navigation technology.
Background
The pedestrian navigation system is an important branch in the technical field of navigation and positioning, and can provide reliable safety guarantee for high-risk workers engaged in fire fighting, emergency rescue and the like. The traditional pedestrian navigation core is GPS, but the GPS is difficult to play a role when being positioned in urban high-rise buildings and crowded forests. The pedestrian positioning system taking the MEMS inertial sensor as the core does not need to consider the problems. Because the MEMS inertial sensor has the advantages of small size, low cost, light weight, etc., the research on the pedestrian navigation of the MEMS sensor has become a hot spot. non-GPS person tracking systems typically use gyroscopes to estimate changes in the heading of a user. Gyroscopes measure rate of rotation and their signals need to be numerically integrated to produce the desired heading information. Due to the small, nearly constant drift value in the measurement signal of the gyroscope, in particular the low accuracy of the MEMS gyroscope, the drift is relatively large, so that a non-negligible error is produced between the calculated heading angle and the true heading angle during the signal integration.
The pedestrian navigation motion track deviates from a real walking route along with the accumulation of course angle errors, and how to reduce the course angle errors to a reasonable range becomes a great problem of the pedestrian navigation research. The commonly used methods at the present stage mainly comprise:
(1) the drift of the gyroscope at each quiescent instant is corrected using a zero angular rate correction algorithm.
(2) And reducing the error of the heading angle by utilizing a heuristic drift reduction technology.
(3) The magnetic heading is easy to interfere but has high long-time positioning accuracy and high short-time positioning accuracy of strong anti-interference capability of the MEMS inertial navigation, and long-time error divergence is complemented with the advantages and disadvantages of the long-time error divergence to correct the MEMS inertial navigation heading.
(4) The heading angle is corrected using the heading information, but based on the assumption that most corridors and paths within the building are straight.
(5) And (3) utilizing WIFI to assist track calculation, and utilizing unscented Kalman filtering to correct the course angle in a loose combination mode.
(6) Zero Velocity update (ZUPT) is used to calibrate the sensor drift errors of the accelerometer and gyroscope.
When the Kalman filtering is used for zero-speed correction, although the speed error and the horizontal attitude angle error can be restrained, the course error angle still cannot be well corrected. Through the observability analysis of the Kalman filtering equation, the observability of the course error angle is very poor, and the Kalman filtering cannot accurately estimate the state quantity of the poor observability.
Disclosure of Invention
The invention aims to provide an MEMS pedestrian navigation method based on a ZIHR course angle correction algorithm, which can improve navigation accuracy.
The purpose of the invention is realized as follows:
the method comprises the following steps:
the method comprises the following steps: performing initial alignment on the MEMS pedestrian navigation system by using the accelerometer and the magnetometer at the static moment;
step two: solving an inertial navigation resolving equation and an error equation of the MEMS pedestrian navigation system;
step three: detecting a zero-speed state by utilizing output values of a gyroscope and an accelerometer;
step four: solving the relation between the heading angle difference value of the adjacent time in the zero-speed state in the ZIHR correction algorithm and the gyro drift and heading error angle;
step five: and establishing a simplified MEMS pedestrian navigation UKF filtering model for UKF filtering.
The present invention may further comprise:
1. the fourth step specifically comprises:
navigation coordinate system sequentially converts phiz，φx，φyObtaining a carrier coordinate system, and obtaining an angular velocity vector omega of the carrier coordinate system relative to a navigation coordinate systemnbWritten in the form of a projection along the carrier coordinate system,
after inverting the matrix of the above equation to:
wherein,the change rate of the real course angle after the bringing is as follows:
the calculated change rate of the course angle is as follows:
whereinIn order to be able to compensate for the drift of the gyroscope,since the gyroscope is in a stationary state, it outputs noiseSubtracting the above two equations yields:
the difference value of the front course angle and the rear course angle is obtained by calculation
Is zero mean white Gaussian noise, and is treated as measurement noise to be simplified into the following formula4；The local latitude is; Δ tkFor the sampling period, the difference value of the course angles at the two sampling moments after simplification is as follows:
2. the fifth step specifically comprises:
selecting the position error in the geographic coordinate system,The speed error, the attitude error, the accelerometer zero offset and the gyroscope constant drift form the state of a state vector forming systemAdding ZIHR correction observation on the basis of zero-speed correction, and measuring the observed quantityδ v is the speed error of the system,the difference value of the course angles of two adjacent sampling moments is as follows:
zk＝Hkxk+vk
wherein F (t)k)、G(tk) All the functions are nonlinear functions and are obtained according to a system error equation; w is system noise, v is measurement noise, w and HkThe values are as follows:
wherein w and v are zero mean white Gaussian noise and E [ w ]k]＝0，E[vk]＝0，δkjIs a Dirac function;
the simplified UKF filtering process is as follows:
(1) initial state variable and its variance
(2) And (3) time updating:
(3) measurement update
Wherein, [ x ]k]LIs a matrix of L columns, each column being xkOther parameters are calculated as follows:
in the formula, L is xkα is a very small number, 10, for determining the distribution of Sigma points around its mean-4α -1, kappa-3-L, β and xkRegarding the distribution form of (c), β ═ 2 is an optimum value for a normal distribution, P represents an error covariance matrix, and K represents a gain matrix.
In order to solve the problem that the course error angle is accumulated along with time in pedestrian navigation, the invention provides a course angle correction algorithm based on ZIHR, one-dimensional measurement is expanded on the basis of zero-speed correction, a simplified UKF filtering model is adopted for the nonlinear characteristics of the system, and the estimated error value is subjected to feedback correction to improve the navigation precision of the original system.
According to the invention, on the basis of Zero-speed correction, a Zero-Integrated heading angle rate (ZIHR) heading angle correction algorithm is added, the true values of the heading angles at two adjacent moments are the same in a Zero state but the heading angles output by an inertial navigation system are different, the difference value of the heading angles at the two adjacent moments is used as a measurement value, formula derivation finds that the determined relation exists between the difference value of the heading angles, the constant drift of a gyroscope and the heading error angle, and experiments prove that the method can well inhibit the divergence of the heading error angle.
The invention relates to a Kalman filtering model, which aims at a linear system, and aims at solving the problem that the nonlinearity of the system model is serious due to the low precision of an MEMS inertial device used for pedestrian navigation. Compared with the linear Kalman filtering, the method has the advantages that the method has the same steps except for using the UT transformation to perform the one-step prediction of the state and the one-step prediction error covariance, but the filtering precision is greatly improved.
The advantages of the invention are mainly reflected in that:
(1) the information of the static moment is used to the maximum extent, the difference value of two adjacent course angles in the zero-speed state is used as the measurement, only one-dimensional measurement is expanded, the calculation amount is not complex, and the divergence of course error angles can be well inhibited.
(2) The problem of divergence of navigation parameter errors of the low-precision MEMS sensor during long-time work can be well restrained by utilizing the UKF filter to perform real-time feedback correction at zero-speed moment, the model errors of the system are reduced, and the positioning precision of a pedestrian navigation system is improved.
Drawings
FIG. 1 is a schematic block diagram of a pedestrian navigation system based on ZIHR course angle correction;
FIG. 2 is a flow chart of a pedestrian navigation system principle based on ZIHR course angle correction;
FIG. 3 is a schematic diagram of accelerometer and gyroscope output values;
FIG. 4 is a comparison graph of course angles with and without the ZIHR correction in a static experiment;
FIG. 5 is a comparison of the standard deviation of heading angle error with and without the addition of ZIHR correction in a static experiment;
FIG. 6 is a schematic of speed and stall condition detection;
FIG. 7 is a graph of a travel trace for two consecutive identical turns with a ZIHR modification;
FIG. 8 is a graph of a two-turn walking trajectory of the same sequence without the ZIHR correction;
FIG. 9 is a schematic view of the change in heading angle with two consecutive passes modified by adding ZIHR.
FIG. 10 is a graph comparing traces for a rectangular walk with and without the addition of ZIHR correction;
FIG. 11 is a comparison plot of a rectangular walk on a hundred degree map with and without a ZIHR modification;
FIG. 12 is a schematic view of the course angle change for a rectangular walk plus a ZIHR correction;
FIG. 13 is a comparison of positioning error with and without the addition of ZIHR correction in Table 1;
FIG. 14 is a table 2 of operating parameters of the MEMS inertial sensor;
Detailed Description
The invention is described in more detail below by way of example:
as shown in FIG. 1, after the initial alignment of the MEMS inertial navigation system, the output values of the gyroscope and the accelerometer are collected by using the data collection software matched with the MEMS. On one hand, the inertial navigation is calculated by utilizing the data of the gyroscope and the accelerometer, and on the other hand, the zero-speed detection is calculated. When no zero speed is detected, the UKF only carries out time updating, and when the zero speed moment is detected, ZIHR correction and zero speed correction are triggered to carry out measurement updating. And correcting the position error, the speed error, the attitude error, the gyro constant drift and the zero offset of the accelerometer estimated by the UKF filter into an MEMS inertial navigation resolving unit through feedback.
The invention discloses an application of a ZIHR-based course angle correction algorithm in MEMS pedestrian navigation, which comprises the following specific steps:
the method comprises the following steps: the initial alignment provides initial attitude information for navigation solution, and is an important link in navigation positioning. The initial alignment, in turn, includes coarse alignment and fine alignment. The MEMS gyroscope cannot be sensitive to the rotational angular velocity of the earth due to low precision, so that the self-calibration function in the direction cannot be realized, and only the output value of the accelerometer at the static moment can be used for realizing horizontal coarse alignment. Calculated pitch angleAnd roll angleThe initial values of (a) are:
wherein,measuring specific force values f for the accelerometers respectivelybThe components in the three axes of the carrier coordinate system. Coarse alignment in azimuth requires the use of external information acquisition, such as a magnetometer. If three-axis magnetometer is arranged under carrier coordinate systemMeasured value isThen the three-axis magnetometer projects under the n' system as:
wherein the n' system and the geographic coordinate system n system differ only by a rotation matrix around the Z axis; mdIs the local magnetic declination;is the heading angle calculated by the magnetometer. Obtained as described aboveThere is also a large error. Therefore, the initial attitude angle is more accurate by using the UKF filter based on ZIHR and zero-speed correction after keeping the time for stillness before navigation solution.
Step two: and taking a geographic coordinate system (an n system) as a navigation system, and taking a carrier coordinate system (a b system) on the right-front-upper part of the human body. The MEMS pedestrian navigation primitive equation is as follows:
wherein,is an attitude matrix;is composed ofAn antisymmetric matrix of (a);is composed ofThe anti-symmetric matrix of (a) is, is the angular velocity due to ground speed; v. ofnIs the projection of the carrier velocity on the navigation system; r isnIs the projection of the pedestrian position on the navigation system. According to the characteristics of low precision of MEMS inertial device and human walkingAndboth terms are ignored. The simplified MEMS pedestrian navigation basic equation is as follows:
the simplified UKF filter model is established based on the error equation of the MEMS inertial navigation system. The system error model of the MEMS strapdown inertial navigation under the geographic coordinate system is as follows:
wherein, δ rn,δvn,Position error, velocity error, attitude error, accelerometer zero offset and gyroscope constant drift respectively;is zero mean white gaussian noise. Since the products of the attitude matrix and the noise are included in the equations (12) and (13), the noise cannot be processed as usual additive.
Step three: the zero speed detection technology is used for judging whether the pedestrian is in a zero speed state within a period of time. The current algorithms mainly comprise two types, one type is detection by using an output value of an MEMS (micro-electromechanical system) inertia measurement element, and the other type mainly comprises hidden Markov model detection, generalized likelihood ratio detection, acceleration variance detection, acceleration modulus detection and angular velocity energy detection. The other is detection by external auxiliary equipment, such as a high-resolution pressure sensor which is arranged at the place where the heel of the pedestrian contacts the ground, and the zero speed is detected according to the pressure measurement value in the gait cycle. Magnetometer detection detects zero velocity and the like from the measurement results of the permanent magnet in each gait cycle. The method of the generalized likelihood ratio is used for zero-speed detection, the output values of the gyroscope and the accelerometer are fused in detection, and the detection precision is greatly improved.
The zero-speed detection is regarded as a hypothetical test problem, a sampling value is N, namely a sliding window is N, if the following formula is established, the hypothesis is established, otherwise, the hypothesis is not established, and the pedestrian is in a motion state.
Wherein,at t for gyroscopes and accelerometerskAn output value of a time; g is the earth gravity acceleration; t (f)k,ωk) Test statistic for zero-speed detection; j is a threshold value; sigmafOutputting a standard deviation of the noise for the accelerometer; sigmaωThe standard deviation of the gyroscope output noise. Threshold J and sumThe output noise statistics of the velocimeter and the gyroscope are related, and the window size N is related to the output frequency and the walking speed of the MEMS.
Step four: in the zero-speed state, if Kalman filtering is performed only by using a zero-speed correction technology, a good correction effect on the course error angle cannot be achieved because the observability of the course error angle is poor. Theoretically, the real course angles at adjacent moments are the same in the zero-speed state, and the difference exists between the course angles at the adjacent moments solved by inertial navigation due to the fact that the drift of the MEMS gyroscope is large. The difference value of the course angles at the adjacent moments in the zero-speed state is used as a measurement value to correct the course angle error caused by the drift of the gyroscope, so that the effect of restraining the drift of the gyroscope can be achieved.
Navigation coordinate system sequentially converts phiz，φx，φyA carrier coordinate system may be obtained. Angular velocity vector omega of carrier coordinate system relative to navigation coordinate systemnbWritten as a projection along the carrier coordinate system.
After inverting the matrix of the above formula, we obtain:
wherein,the change rate of the real course angle after the bringing is as follows:
the calculated change rate of the course angle is as follows:
whereinIn order for the gyroscope to drift in a constant value,is zero mean white gaussian noise. Because it is in a static state, thereforeSubtracting the above two equations yields:
because of the fact thatThen the difference between the front and rear heading angles obtained by calculation is:
is zero mean white Gaussian noise, and is treated as measurement noise to be simplified into the following formula4；The local latitude is; Δ tkIs the sampling period. The difference between the course angles at the two sampling moments after simplification is as follows:
step five: selecting position error, speed error, attitude error, accelerometer zero bias and gyroscope constant drift in the geographic coordinate system to form a state vector, namely the state of the systemAt the zero-speed moment, if the course error angle divergence cannot be restrained by only correcting the observed quantity by the zero-speed, and in order to improve the accuracy of the course angle, the ZIHR correction observation is added on the basis of the zero-speed correction, the observed quantityδvnIs the speed error of the system and is,the difference value of the course angles of two adjacent sampling moments is obtained. The system state equation and the measurement equation are as follows:
zk＝Hkxk+vk(31)
wherein F (t)k)，G(tk) All are nonlinear functions, which can be obtained according to a system error equation, wherein w is system noise, v is measurement noise, and w and HkThe values are as follows:
wherein w and v are zero mean white Gaussian noise and E [ w ]k]＝0，E[vk]＝0，δkjAs a Dirac function, QkIs a system noise sequence variance matrix, RkIs a variance matrix of the measured noise sequence.
Simplified UKF filtering process:
(3) initial state variables and their variances:
(4) and (3) time updating:
(3) measurement updating:
wherein, [ x ]k]LIs a matrix of L columns, each column being xkOther parameters are calculated as follows:
in the formula, L is xkα is used to determine the distribution of Sigma points around its mean, is a very small number, and may take 10-4α -1, kappa-3-L, β and xkThe distribution form of (a) is related to (b), wherein β is an optimal value when the distribution is normal, P represents an error covariance matrix, and K represents a gain matrix.
Verification example:
the MIMU used in the experiment of the present invention is MTi-G series MEMS produced by XSENS. The device has the biggest characteristics of small volume, light weight, stable work and capability of ensuring certain precision and completely meeting the requirement of individual navigation. The MTi-G type MEMS comprises a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer which are used in an algorithm. The operating parameters of the MEMS inertial sensor are listed in table 2 of fig. 14.
Experimental verification and result analysis, and setting standard deviation of output noise of accelerometer as sigmaf0.02m/s, standard deviation of gyroscope output noise σw0.1 × pi/180 rad/s, the sampling frequency is set to 100Hz, the sliding window size N is set to 3, and the threshold for the generalized likelihood ratio determination of motion and standstill is 0.3 × 105. Initial value of state x0Average value is 0, initial prediction error covariance matrixSystem noise variance matrixMeasurement noise variance matrix
(1) The first set of experiments: data was collected for 14min on a seismic isolation station. The course angle with the ZIHR correction in fig. 4 initially diverges faster than the course error angle without the ZIHR correction, but then does not increase and slowly returns to the initial value because the ZIHR correction algorithm is constraining to the divergence of the course error angle. The course angle without the ZIHR correction will increase linearly with a certain slope, the greater the difference from the initial value. FIG. 5 is a comparison graph of the standard deviation of the heading angle with and without the ZIHR modification obtained by Kalman filtering, where the standard deviation of the heading angle without the ZIHR modification is about 10 degrees, the standard deviation of the heading error with the ZIHR modification is about 5 degrees, and the relationship is about two times.
(2) The second set of experiments: walking for two circles continuously, walking for 70 meters and 70 steps, and walking time is 85 seconds. Fig. 6 shows the generalized likelihood ratio zero-speed detection result, and it is apparent from the figure that the zero-speed detection value is 1 when the speed is zero, which shows that the generalized likelihood ratio zero-speed detection model is very accurate. FIG. 7 is a graph of two consecutive identical circles of travel with the ZIHR modification added, it can be seen that the two circles of travel are substantially coincident, and the start point and the end point are also substantially coincident, and the horizontal distance between the two points differs by only 0.19 m. FIG. 8 is a diagram of two consecutive identical turns of the travel trajectory without the ZIHR correction, and it can be seen that the trajectory becomes rough due to small variations in the heading angle, and that the two turns of the trajectory just start to substantially coincide, and then vary by a large distance due to the heading angle. And the distance between the starting point and the ending point is also far, differing by 1.65 meters. FIG. 9 is a graph of the change in heading angle for two consecutive identical turns with the ZIHR correction applied, and it can be seen that the heading angle changes regularly, with a 90 degree change for each turn.
(3) The third set of experiments: a building walks a closed rectangle with a walking distance of 415 meters, wherein the red line is a locus diagram with the ZIHR correction added, and the blue line is a locus diagram without the ZIHR correction added, and it is obvious from fig. 10 that the course angle can be corrected in time at the last distance after the ZIHR correction is added. In fig. 11, the hundredth map is used as a reference for a real track, and the calculated track is found to be matched with the position of the building in the hundredth map, which indicates that the calculated track is not shifted as a whole. FIG. 12 is a schematic view of the change in heading angle after the ZIHR correction, which was found to be regular with a 90 degree change per turn.
And (4) conclusion:
the invention provides a ZIHR algorithm to correct a course error angle on the basis of a UKF filter based on zero-speed correction, and solves the problem that the course error angle is easy to diverge in a low-precision MEMS inertial navigation algorithm to a certain extent. From simulation results, the zero-speed moment is accurately detected by utilizing the rule of human walking, ZIHR correction and zero-speed correction are carried out by utilizing simplified UKF filtering during the zero-speed period, most of horizontal attitude errors and speed errors can be calibrated by the algorithm, and a course error angle and a position error are well inhibited compared with a course error angle and a position error which are only obtained by using a zero-speed correction algorithm, so that the positioning precision of the MEMS inertial navigation system is improved.
Claims (3)
1. An MEMS pedestrian navigation method based on a ZIHR course angle correction algorithm is characterized in that:
the method comprises the following steps: performing initial alignment on the MEMS pedestrian navigation system by using the accelerometer and the magnetometer at the static moment;
step two: solving an inertial navigation resolving equation and an error equation of the MEMS pedestrian navigation system;
step three: detecting a zero-speed state by utilizing output values of a gyroscope and an accelerometer;
step four: solving the relation between the heading angle difference value of the adjacent time in the zero-speed state in the ZIHR correction algorithm and the gyro drift and heading error angle;
step five: and establishing a simplified MEMS pedestrian navigation UKF filtering model for UKF filtering.
2. The MEMS pedestrian navigation method based on the ZIHR course angle correction algorithm as claimed in claim 1, wherein the fourth step specifically includes:
navigation coordinate system sequentially converts phiz，φx，φyObtaining a carrier coordinate system, and obtaining an angular velocity vector omega of the carrier coordinate system relative to a navigation coordinate systemnbWritten in the form of a projection along the carrier coordinate system,
after inverting the matrix of the above equation to:
wherein,the change rate of the real course angle after the bringing is as follows:
the calculated change rate of the course angle is as follows:
whereinIn order to be able to compensate for the drift of the gyroscope,since the gyroscope is in a stationary state, it outputs noiseSubtracting the above two equations yields:
the difference value of the front course angle and the rear course angle is obtained by calculation
Is zero mean white Gaussian noise, and is treated as measurement noise to be simplified into the following formula4；The local latitude is; Δ tkFor the sampling period, the difference value of the course angles at the two sampling moments after simplification is as follows:
3. the MEMS pedestrian navigation method based on the ZIHR course angle correction algorithm according to claim 1 or 2, wherein the fifth step specifically includes:
selecting position error, speed error, attitude error, accelerometer zero bias and gyroscope constant drift in the geographic coordinate system to form the state of the state vector forming systemAdding ZIHR correction observation on the basis of zero-speed correction, and measuring the observed quantityδ v is the speed error of the system,the difference value of the course angles of two adjacent sampling moments is as follows:
zk＝Hkxk+vk
wherein F (t)k)、G(tk) All the functions are nonlinear functions and are obtained according to a system error equation; w is system noise, v is measurement noise, w and HkThe values are as follows:
wherein w and v are zero mean white Gaussian noise and E [ w ]k]＝0，E[vk]＝0，δkjIs a Dirac function;
the simplified UKF filtering process is as follows:
(1) initial state variable and its variance
(2) And (3) time updating:
(3) measurement update
Wherein, [ x ]k]LIs a matrix of L columns, each column being xkOther parameters are calculated as follows:
in the formula, L is xkα is a very small number, 10, for determining the distribution of Sigma points around its mean-4α -1, kappa-3-L, β and xkRegarding the distribution form of (c), β ═ 2 is an optimum value for a normal distribution, P represents an error covariance matrix, and K represents a gain matrix.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810107852.9A CN108426574A (en) | 2018-02-02 | 2018-02-02 | A kind of MEMS pedestrian navigation methods of the course angle correction algorithm based on ZIHR |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810107852.9A CN108426574A (en) | 2018-02-02 | 2018-02-02 | A kind of MEMS pedestrian navigation methods of the course angle correction algorithm based on ZIHR |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108426574A true CN108426574A (en) | 2018-08-21 |
Family
ID=63156438
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810107852.9A Pending CN108426574A (en) | 2018-02-02 | 2018-02-02 | A kind of MEMS pedestrian navigation methods of the course angle correction algorithm based on ZIHR |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108426574A (en) |
Cited By (29)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109579838A (en) * | 2019-01-14 | 2019-04-05 | 湖南海迅自动化技术有限公司 | The localization method and positioning system of AGV trolley |
| CN109612464A (en) * | 2018-11-23 | 2019-04-12 | 南京航空航天大学 | Indoor navigation system and method based on more algorithms enhancing under IEZ frame |
| CN109612460A (en) * | 2018-12-19 | 2019-04-12 | 东南大学 | One kind being based on static modified deviation of plumb line measurement method |
| CN109764865A (en) * | 2019-01-25 | 2019-05-17 | 北京交通大学 | A kind of indoor orientation method based on MEMS and UWB |
| CN109855621A (en) * | 2018-12-27 | 2019-06-07 | 国网江苏省电力有限公司检修分公司 | A kind of composed chamber's one skilled in the art's navigation system and method based on UWB and SINS |
| CN109883423A (en) * | 2019-02-25 | 2019-06-14 | 广州市香港科大霍英东研究院 | Localization method, system, equipment and storage medium based on Kalman filtering |
| CN109916398A (en) * | 2019-03-22 | 2019-06-21 | 南京航空航天大学 | A kind of adaptive quaternary number particle filter attitude data fusion method |
| CN110057381A (en) * | 2019-03-20 | 2019-07-26 | 深圳市元征科技股份有限公司 | A kind of zero velocity update method and system of navigation system |
| CN110553646A (en) * | 2019-07-30 | 2019-12-10 | 南京林业大学 | Pedestrian navigation method based on inertia, magnetic heading and zero-speed correction |
| CN110764506A (en) * | 2019-11-05 | 2020-02-07 | 广东博智林机器人有限公司 | Course angle fusion method and device of mobile robot and mobile robot |
| CN110763228A (en) * | 2019-10-08 | 2020-02-07 | 哈尔滨工程大学 | Error correction method of integrated navigation system based on seabed oil and gas pipe node position |
| CN111024070A (en) * | 2019-12-23 | 2020-04-17 | 哈尔滨工程大学 | Inertial foot binding type pedestrian positioning method based on course self-observation |
| CN111024126A (en) * | 2019-12-26 | 2020-04-17 | 北京航天控制仪器研究所 | Self-adaptive zero-speed correction method in pedestrian navigation positioning |
| CN111189443A (en) * | 2020-01-14 | 2020-05-22 | 电子科技大学 | Pedestrian navigation method for online step length calibration, motion deviation angle correction and adaptive energy management |
| CN111366154A (en) * | 2020-03-26 | 2020-07-03 | 湖南三一快而居住宅工业有限公司 | Course angle determining method and device and electronic equipment |
| CN111678513A (en) * | 2020-06-18 | 2020-09-18 | 山东建筑大学 | Ultra-wideband/inertial navigation tight coupling indoor positioning device and system |
| CN111750863A (en) * | 2020-06-18 | 2020-10-09 | 哈尔滨工程大学 | Navigation system error correction method based on auxiliary position of sea pipe node |
| CN112197767A (en) * | 2020-10-10 | 2021-01-08 | 江西洪都航空工业集团有限责任公司 | Filter design method for improving filtering error on line |
| CN112197791A (en) * | 2020-09-15 | 2021-01-08 | 武汉华中天纬测控有限公司 | Method for correcting drift error of isolating switch monitoring equipment |
| CN112362057A (en) * | 2020-10-26 | 2021-02-12 | 中国人民解放军海军航空大学 | Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation |
| CN112379319A (en) * | 2020-11-18 | 2021-02-19 | 北京自动化控制设备研究所 | Course error testing device for atomic magnetometer |
| CN113472318A (en) * | 2021-07-14 | 2021-10-01 | 青岛杰瑞自动化有限公司 | Hierarchical self-adaptive filtering method and system considering observation model errors |
| CN113483753A (en) * | 2021-06-30 | 2021-10-08 | 北京航空航天大学 | Inertial heading error elimination method based on environmental constraint |
| CN114370885A (en) * | 2021-10-29 | 2022-04-19 | 北京自动化控制设备研究所 | Error compensation method and system for inertial navigation system |
| CN114413932A (en) * | 2022-01-03 | 2022-04-29 | 中国电子科技集团公司第二十研究所 | Positioning error correction test method based on communication between vehicle-mounted platforms |
| CN114964231A (en) * | 2022-05-12 | 2022-08-30 | 北京自动化控制设备研究所 | Vehicle-mounted inertia/satellite combined navigation method based on double-channel course matching |
| CN115235513A (en) * | 2022-09-15 | 2022-10-25 | 中国船舶重工集团公司第七0七研究所 | Inertial navigation correction method based on satellite navigation pseudo range and pseudo range rate |
| CN117848324A (en) * | 2023-12-29 | 2024-04-09 | 河北美泰电子科技有限公司 | Wearable MEMS inertial sensor calibration method and device |
| CN118089745A (en) * | 2024-04-26 | 2024-05-28 | 北京航宇测通电子科技有限公司 | MIMU course angle precision optimization and adjustment system and method based on geomagnetic matching |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103776446A (en) * | 2013-10-29 | 2014-05-07 | 哈尔滨工程大学 | Pedestrian autonomous navigation calculation algorithm based on MEMS-IMU |
| CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
-
2018
- 2018-02-02 CN CN201810107852.9A patent/CN108426574A/en active Pending
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103776446A (en) * | 2013-10-29 | 2014-05-07 | 哈尔滨工程大学 | Pedestrian autonomous navigation calculation algorithm based on MEMS-IMU |
| CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
Non-Patent Citations (3)
| Title |
|---|
| 徐海鑫等: "零角度修正在GNSS/INS组合导航中的应用", 《测绘科学技术学报》 * |
| 徐海鑫等: "零速修正在GNSS/INS组合导航中的应用", 《全球定位系统》 * |
| 赵红宇: "惯性行人的导航系统的算法研究", 《中国博士学位论文全文数据库•信息科技辑》 * |
Cited By (41)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109612464B (en) * | 2018-11-23 | 2022-06-10 | 南京航空航天大学 | Multi-algorithm enhanced indoor navigation system and method based on IEZ framework |
| CN109612464A (en) * | 2018-11-23 | 2019-04-12 | 南京航空航天大学 | Indoor navigation system and method based on more algorithms enhancing under IEZ frame |
| CN109612460A (en) * | 2018-12-19 | 2019-04-12 | 东南大学 | One kind being based on static modified deviation of plumb line measurement method |
| CN109855621A (en) * | 2018-12-27 | 2019-06-07 | 国网江苏省电力有限公司检修分公司 | A kind of composed chamber's one skilled in the art's navigation system and method based on UWB and SINS |
| CN109579838A (en) * | 2019-01-14 | 2019-04-05 | 湖南海迅自动化技术有限公司 | The localization method and positioning system of AGV trolley |
| CN109764865A (en) * | 2019-01-25 | 2019-05-17 | 北京交通大学 | A kind of indoor orientation method based on MEMS and UWB |
| CN109764865B (en) * | 2019-01-25 | 2022-11-04 | 北京交通大学 | Indoor positioning method based on MEMS and UWB |
| CN109883423A (en) * | 2019-02-25 | 2019-06-14 | 广州市香港科大霍英东研究院 | Localization method, system, equipment and storage medium based on Kalman filtering |
| CN110057381A (en) * | 2019-03-20 | 2019-07-26 | 深圳市元征科技股份有限公司 | A kind of zero velocity update method and system of navigation system |
| CN109916398A (en) * | 2019-03-22 | 2019-06-21 | 南京航空航天大学 | A kind of adaptive quaternary number particle filter attitude data fusion method |
| CN110553646A (en) * | 2019-07-30 | 2019-12-10 | 南京林业大学 | Pedestrian navigation method based on inertia, magnetic heading and zero-speed correction |
| CN110553646B (en) * | 2019-07-30 | 2023-03-21 | 南京林业大学 | Pedestrian navigation method based on inertia, magnetic heading and zero-speed correction |
| CN110763228A (en) * | 2019-10-08 | 2020-02-07 | 哈尔滨工程大学 | Error correction method of integrated navigation system based on seabed oil and gas pipe node position |
| CN110764506A (en) * | 2019-11-05 | 2020-02-07 | 广东博智林机器人有限公司 | Course angle fusion method and device of mobile robot and mobile robot |
| CN110764506B (en) * | 2019-11-05 | 2022-10-11 | 广东博智林机器人有限公司 | Course angle fusion method and device of mobile robot and mobile robot |
| CN111024070A (en) * | 2019-12-23 | 2020-04-17 | 哈尔滨工程大学 | Inertial foot binding type pedestrian positioning method based on course self-observation |
| CN111024126A (en) * | 2019-12-26 | 2020-04-17 | 北京航天控制仪器研究所 | Self-adaptive zero-speed correction method in pedestrian navigation positioning |
| CN111024126B (en) * | 2019-12-26 | 2022-03-22 | 北京航天控制仪器研究所 | Self-adaptive zero-speed correction method in pedestrian navigation positioning |
| CN111189443A (en) * | 2020-01-14 | 2020-05-22 | 电子科技大学 | Pedestrian navigation method for online step length calibration, motion deviation angle correction and adaptive energy management |
| CN111366154A (en) * | 2020-03-26 | 2020-07-03 | 湖南三一快而居住宅工业有限公司 | Course angle determining method and device and electronic equipment |
| CN111366154B (en) * | 2020-03-26 | 2022-05-17 | 三一建筑机器人(西安)研究院有限公司 | Course angle determining method and device and electronic equipment |
| CN111750863A (en) * | 2020-06-18 | 2020-10-09 | 哈尔滨工程大学 | Navigation system error correction method based on auxiliary position of sea pipe node |
| CN111678513A (en) * | 2020-06-18 | 2020-09-18 | 山东建筑大学 | Ultra-wideband/inertial navigation tight coupling indoor positioning device and system |
| CN112197791A (en) * | 2020-09-15 | 2021-01-08 | 武汉华中天纬测控有限公司 | Method for correcting drift error of isolating switch monitoring equipment |
| CN112197767A (en) * | 2020-10-10 | 2021-01-08 | 江西洪都航空工业集团有限责任公司 | Filter design method for improving filtering error on line |
| CN112362057A (en) * | 2020-10-26 | 2021-02-12 | 中国人民解放军海军航空大学 | Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation |
| CN112379319A (en) * | 2020-11-18 | 2021-02-19 | 北京自动化控制设备研究所 | Course error testing device for atomic magnetometer |
| CN113483753B (en) * | 2021-06-30 | 2022-11-01 | 北京航空航天大学 | Inertial course error elimination method based on environmental constraint |
| CN113483753A (en) * | 2021-06-30 | 2021-10-08 | 北京航空航天大学 | Inertial heading error elimination method based on environmental constraint |
| CN113472318A (en) * | 2021-07-14 | 2021-10-01 | 青岛杰瑞自动化有限公司 | Hierarchical self-adaptive filtering method and system considering observation model errors |
| CN113472318B (en) * | 2021-07-14 | 2024-02-06 | 青岛杰瑞自动化有限公司 | Hierarchical self-adaptive filtering method and system considering observation model errors |
| CN114370885A (en) * | 2021-10-29 | 2022-04-19 | 北京自动化控制设备研究所 | Error compensation method and system for inertial navigation system |
| CN114370885B (en) * | 2021-10-29 | 2023-10-13 | 北京自动化控制设备研究所 | Error compensation method and system for inertial navigation system |
| CN114413932B (en) * | 2022-01-03 | 2024-05-14 | 中国电子科技集团公司第二十研究所 | Positioning error correction testing method based on communication between vehicle-mounted platforms |
| CN114413932A (en) * | 2022-01-03 | 2022-04-29 | 中国电子科技集团公司第二十研究所 | Positioning error correction test method based on communication between vehicle-mounted platforms |
| CN114964231A (en) * | 2022-05-12 | 2022-08-30 | 北京自动化控制设备研究所 | Vehicle-mounted inertia/satellite combined navigation method based on double-channel course matching |
| CN114964231B (en) * | 2022-05-12 | 2024-08-06 | 北京自动化控制设备研究所 | Vehicle-mounted inertial/satellite integrated navigation method based on dual-channel course matching |
| CN115235513A (en) * | 2022-09-15 | 2022-10-25 | 中国船舶重工集团公司第七0七研究所 | Inertial navigation correction method based on satellite navigation pseudo range and pseudo range rate |
| CN117848324A (en) * | 2023-12-29 | 2024-04-09 | 河北美泰电子科技有限公司 | Wearable MEMS inertial sensor calibration method and device |
| CN118089745A (en) * | 2024-04-26 | 2024-05-28 | 北京航宇测通电子科技有限公司 | MIMU course angle precision optimization and adjustment system and method based on geomagnetic matching |
| CN118089745B (en) * | 2024-04-26 | 2024-07-02 | 北京航宇测通电子科技有限公司 | MIMU course angle precision optimization and adjustment system and method based on geomagnetic matching |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108426574A (en) | A kind of MEMS pedestrian navigation methods of the course angle correction algorithm based on ZIHR | |
| CN105043385B (en) | A kind of method for adaptive kalman filtering of pedestrian's Camera calibration | |
| CN104061934B (en) | Pedestrian indoor position tracking method based on inertial sensor | |
| CN111024075B (en) | Pedestrian navigation error correction filtering method combining Bluetooth beacon and map | |
| Ladetto et al. | Combining gyroscopes, magnetic compass and GPS for pedestrian navigation | |
| CN107490378B (en) | Indoor positioning and navigation method based on MPU6050 and smart phone | |
| CN103616030A (en) | Autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction | |
| CN104713554A (en) | Indoor positioning method based on MEMS insert device and android smart mobile phone fusion | |
| CN107014388B (en) | Pedestrian trajectory calculation method and device based on magnetic interference detection | |
| Benzerrouk et al. | Robust IMU/UWB integration for indoor pedestrian navigation | |
| Chen et al. | IMU/GPS based pedestrian localization | |
| Luo et al. | A position loci-based in-motion initial alignment method for low-cost attitude and heading reference system | |
| Anbu et al. | Integration of inertial navigation system with global positioning system using extended kalman filter | |
| Woyano et al. | Evaluation and comparison of performance analysis of indoor inertial navigation system based on foot mounted IMU | |
| CN103278165A (en) | Remanence-calibration-based autonomous navigation method of magnetic survey and starlight backup based on | |
| CN110672095A (en) | Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation | |
| Guo et al. | Robust fading cubature Kalman filter and its application in initial alignment of SINS | |
| Guo et al. | The usability of MTI IMU sensor data in PDR indoor positioning | |
| Luna et al. | An indoor pedestrian positioning system based on inertial measurement unit and wireless local area network | |
| CN106643713B (en) | Estimation method and device for zero-speed correction pedestrian locus for smooth and adaptive adjustment of threshold | |
| KR101301462B1 (en) | Pedestiran dead-reckoning apparatus using low cost inertial measurement unit and method thereof | |
| Cucci et al. | An analysis of a gyro-free inertial system for INS/GNSS navigation | |
| Kubo et al. | DGPS/INS/VVheelSensor Integrationfor High Accuracy Land-Vehicle Positioning | |
| Tian et al. | A pedestrian navigation system based on MEMS inertial measurement unit | |
| CN110567456A (en) | BDS/INS combined train positioning method based on robust Kalman filtering |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180821 |
|
| RJ01 | Rejection of invention patent application after publication |