CN107655476B - Pedestrian high-precision foot navigation method based on multi-information fusion compensation - Google Patents
Pedestrian high-precision foot navigation method based on multi-information fusion compensation Download PDFInfo
- Publication number
- CN107655476B CN107655476B CN201710716885.9A CN201710716885A CN107655476B CN 107655476 B CN107655476 B CN 107655476B CN 201710716885 A CN201710716885 A CN 201710716885A CN 107655476 B CN107655476 B CN 107655476B
- Authority
- CN
- China
- Prior art keywords
- speed
- zero
- error
- navigation
- pedestrian
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 25
- 230000004927 fusion Effects 0.000 title claims abstract description 17
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 79
- 238000005259 measurement Methods 0.000 claims abstract description 47
- 238000001514 detection method Methods 0.000 claims abstract description 26
- 238000012937 correction Methods 0.000 claims abstract description 21
- 238000001914 filtration Methods 0.000 claims abstract description 21
- 238000004364 calculation method Methods 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 24
- 230000008569 process Effects 0.000 claims description 16
- 230000003068 static effect Effects 0.000 claims description 15
- 230000010354 integration Effects 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000009471 action Effects 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 2
- 230000006870 function Effects 0.000 description 4
- 238000005034 decoration Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000003909 pattern recognition Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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/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/18—Stabilised platforms, e.g. by gyroscope
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 invention discloses a pedestrian high-precision foot navigation method based on multi-information fusion compensation, which comprises the steps of collecting original measurement data of a gyroscope and an accelerometer in real time, enabling the error compensation of the initial gyroscope to be zero and the error compensation of the accelerometer to be also zero, and carrying out strapdown calculation through a strapdown inertial navigation calculation algorithm; on the basis, a Kalman filter correction model is constructed for filtering correction, the state equations are consistent, the measurement equations are dynamically adjusted along with multidimensional measurement information, corresponding algorithms are constructed under different multidimensional information detection, the measurement equations are transformed in real time, and the errors of the navigation positioning result and the errors of the sensor are estimated and corrected, and the errors of the gyroscope and the accelerometer are returned; finally, outputting a navigation positioning result, namely the position, the speed and the posture of the pedestrian; by adopting the low-precision consumption-level sensor chip and the magnetic sensor, the problems of high-precision pedestrian positioning and high-long-term course divergence in the GNSS environment without a global navigation satellite system are solved.
Description
Technical Field
The invention relates to a high-precision pedestrian foot navigation algorithm, in particular to a pedestrian high-precision foot navigation method based on multi-information fusion compensation, and belongs to the technical field of pedestrian navigation.
Background
Currently, the research on pedestrian navigation technologies is mainly divided into the following categories: wireless perception, pattern recognition, inertial sensors. The wireless sensing technology needs to deploy additional radio frequency equipment including WIFI, UWB and the like, the application range is narrow, signals are easy to be shielded, and the cost is high; the inertial sensor has the characteristics of autonomy, no external interference and low cost, and is suitable for wide application. However, the low-cost inertial sensor has a large error, and if compensation or other means are not performed, an error of more than a meter level can be caused within a range of several seconds. The method is characterized in that a zero-speed Kalman filtering algorithm based on an inertial navigation system is designed by utilizing a multi-information fusion compensation mechanism, a course error self-observation algorithm of magnetic course stability of a pedestrian under an initial static state is researched, a zero-speed course error self-observation algorithm of the pedestrian under a motion state is researched, a fusion algorithm based on geomagnetic matching and a strapdown inertial navigation algorithm is researched, and high precision and high reliability of the pedestrian under high and long periods are guaranteed. The method is suitable for pedestrian navigation, solves the problem of high-precision pedestrian positioning in the GNSS environment without a global navigation satellite system, can realize high-and-long-term autonomous positioning of pedestrians by using a low-cost sensor, and has high engineering application and commercial value.
Disclosure of Invention
The purpose of the invention is as follows: the invention aims to solve the technical problem of designing a multi-information fusion compensation algorithm suitable for pedestrian high-precision foot navigation by adopting a low-precision inertial sensor and a magnetic sensor.
The technical scheme is as follows:
a pedestrian high-precision foot navigation method based on multi-information fusion compensation is characterized by comprising the following steps: acquiring original measurement data of a gyroscope and an accelerometer in real time, wherein the error compensation of the initial gyroscope is zero, the error compensation of the accelerometer is also zero, and strapdown calculation is carried out through a strapdown inertial navigation calculation algorithm; on the basis, a Kalman filter correction model is constructed for filtering correction, the state equations are consistent, the measurement equations are dynamically adjusted along with multidimensional measurement information, corresponding algorithms are constructed under different multidimensional information detection, the measurement equations are transformed in real time, and the errors of the navigation positioning result and the errors of the sensor are estimated and corrected, and the errors of the gyroscope and the accelerometer are returned; finally, outputting a navigation positioning result, namely the position, the speed and the posture of the pedestrian;
when only zero-speed detection is successful, adopting pseudo measurement information to construct a correction model algorithm based on zero-speed Kalman filtering; when zero-speed detection is successful and initial magnetic heading error self-observation is carried out, a magnetic heading error self-observation algorithm in the pedestrian initial static state is established; when zero-speed detection is successful and zero-speed course error self-observation is carried out in a dynamic state, a zero-speed course error self-observation algorithm in a pedestrian motion state is established; when zero-speed detection is successful, and zero-speed heading error self-observation and geomagnetic matching are carried out in a motion state, a fusion algorithm based on geomagnetic matching and strapdown inertial navigation resolving algorithms is established, wherein a Kalman filter correction model is established under the combined action of a state equation and a measurement equation of each algorithm.
Further: the strapdown inertial navigation resolving algorithm is as follows:
the inertial sensor consists of an accelerometer and a gyroscope, the accelerometer acquires specific force information of a motion carrier, the speed can be obtained through primary integration, and the position can be obtained through secondary integration; the angular velocity of a gyroscope measuring machine system relative to an inertial system can be obtained through one-time integration; the most basic strapdown inertial navigation resolving algorithm comprises attitude resolving, speed resolving and position resolving; the attitude, velocity, position are then calculated from the following equations:
wherein b is a coordinate system of the body, n is a navigation coordinate system, e is a terrestrial coordinate system, i is an inertial coordinate system, f is a specific force, g is a gravitational acceleration,
is a posture transfer matrix from a body coordinate system to a navigation coordinate system,
is the projection of the x, y and z three-axis machine system on the machine system relative to the angular velocity of the inertial system,
for the projection of the three-axis navigation system on the machine system relative to the angular velocity of the inertial system,
is the projection of the angular rate of rotation of the earth under the navigation system,
a column vector formed by the component of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the axial direction of the navigation coordinate system, and v is threeAn axial velocity vector, p being a three-axis position vector;
is composed of
With respect to the derivative of the time t,
is v isnWith respect to the derivative of the time t,
is pnThe derivative with respect to time t, and therefore the above equation, is an ordinary differential equation, which is solved iteratively on the basis of the knowledge of the initial attitude, velocity, position.
And further: the state equation of the Kalman filtering correction model is as follows:
the Kalman filtering model selects a geographical coordinate system of 'northeast sky', and an 18-order state model is constructed as follows:
wherein δ Φ is [ δ Φ ]e δφn δφu]Is the platform angle error; δ ν ═ δ ve δvn δvu]Is the speed error; δ p ═ δ λ δ L δ h]The position error is longitude, latitude and altitude error; epsilonb＝[εbx εby εbz]Is a triaxial gyro random constant; epsilonr＝[εrx εry εrz]Is a first order Markov process of a three axis gyroscope +r＝[▽rx ▽ry ▽rz]A first order Markov process for a three axis accelerometer;
under different geographic coordinate systems
The attitude transition matrixes are different, A and A in the state equation,G. The W coefficient is also different, and in the geographic coordinate system of the northeast, A, G, W is set as shown in the following four formulas: a is the system matrix, G is the system noise matrix, and W is [ omega ]gx ωgy ωgz ωrx ωry ωrz ωax ωay ωaz]The system noise is set according to the characteristics of the sensor; wherein ω isgIs a random white noise drift of a triaxial gyro with a mean square error of sigmag，ωrThe mean square error of the three-axis gyroscope is sigmarIs driven by the Markov process ofaThe mean square error of the triaxial accelerometer is sigmaaThe markov process of (1) drives white noise;
Further: the correction model algorithm based on the zero-speed Kalman filtering is as follows:
when only zero-speed detection is successful, constructing a six-dimensional measurement model in a zero-speed state by using the pseudo measurement information; let veloP be a zero matrix of 3 × 1 order, posip (t) posiN (t-1), and based on the zero-speed kalman filter measurement model, the following formula is shown:
wherein Z is1(t) observed quantities of speed and position, veloN is the real-time resolving speed of the strapdown inertial navigation system, posiN is the real-time resolving position of the strapdown inertial navigation system, MvFor speed error of pedestrian at zero speed, MpFor the position error of the pedestrian at zero speed, V1For measuring noise, H1For the measurement matrix, the following two equations are shown:
Rmradius of curvature of the earth in the meridian, RnThe radius of curvature of the earth is expressed by the unitary-mortise circle, and L is latitude.
And further: the magnetic heading error self-observation algorithm in the pedestrian initial static state is as follows:
under the initial static state of the pedestrian, the magnetic heading angle has stronger stability, so when zero-speed detection succeeds and the initial magnetic heading error is observed, a one-dimensional measurement equation is constructed, which is shown as the following formula:
wherein Z is2(t) is the observed quantity of the initial heading angle,. psiINSThe course angle resolved for strapdown can be calculated by strapdown inertial navigation
The matrix is obtained by trigonometric function conversion
Then
ψmgThe magnetic heading angle can be obtained by resolving magnetic information measured by a magnetic sensor,
resolving the error for the magnetic heading angle, V2To measure noise;
in the initial static state of the pedestrian, the sensor error is further estimated by the following formula:
Z3(t) observations of speed, position and initial heading angle.
And further: the zero-speed course error self-observation algorithm in the pedestrian motion state is as follows:
under the moving state of the pedestrian, when each step goes through a section of zero-speed moment, the course is basically unchanged in the zero-speed stage, and based on the zero-speed course invariant theory, when zero-speed detection is successful and zero-speed course error self-observation is carried out in the dynamic state, the course angle of the first zero-speed time point of the step is recorded as psik-initialRecording the course angle of the rest zero-speed time points in the step as psik-others；
The one-dimensional zero-speed course self-observation measurement equation is constructed as follows:
wherein: z4(t) is the observed quantity of the heading angle in the motion state,
is the course angle error;
is at zero speedCourse angle error, V4To measure noise; thus:
theta is a pitch angle and can be calculated in a strapdown inertial navigation resolving algorithm
The matrix is obtained by performing a trigonometric function conversion,
thus, the 7-dimensional measurement model added with the zero-speed heading error self-observation algorithm in the motion state is as follows:
wherein Z is5And (t) the observed quantity of the heading angle under the states of speed, position and motion.
And further: the fusion algorithm based on the geomagnetic matching and the strapdown navigation resolving algorithm is as follows:
when zero-speed and geomagnetic matching are detected at the same time successfully, a 10-dimensional measurement equation is constructed by using a centralized filter to assist in correcting position errors, speed errors and sensor errors, as shown in the following three formulas, wherein posiM is a geomagnetic matching position and can be directly obtained by geomagnetic matching, and M is a geomagnetic matching position6For geomagnetic matching of position errors, V6For measuring noise, posiN is a position solved by inertial navigation in real time, δ p is a position error, and HI is shown in a correction model algorithm based on zero-speed Kalman filtering:
Z6(t)＝[posiN-posiM]＝[δp+M6]＝H6(t)X(t)+V6(t)
H6＝[03×9 HI 03×9]
Z6(t) is geomagnetism matching position observed quantity, Z7And (t) the speed, the position, the heading angle in the motion state and the geomagnetic matching position observed quantity.
Has the advantages that:
1. the problem of high-precision pedestrian positioning in the GNSS environment without a global satellite navigation positioning system is solved by adopting a low-precision consumption-level sensor chip.
2. By adopting the magnetic sensor, the problem of high and long term course divergence is solved by utilizing the magnetic course angle and the geomagnetic matching.
3. The inertial sensor and the magnetic sensor have low cost and wide popularization, and the practicability and the popularization of the algorithm are stronger.
Drawings
FIG. 1 is a flow chart of a pedestrian high-precision foot navigation method based on multi-information fusion compensation.
Detailed Description
The invention is further explained below with reference to the drawings.
The present invention will be better understood from the following examples. However, those skilled in the art will readily appreciate that the specific material ratios, process conditions and results thereof described in the examples are illustrative only and should not be taken as limiting the invention as detailed in the claims.
As shown in fig. 1, on the basis of the known initial position, speed and posture of the pedestrian, acquiring information of an accelerometer and a gyroscope in real time to perform strapdown inertial navigation calculation, and obtaining the speed, position and posture of the inertial navigation calculation; carrying out zero-speed detection and constructing pseudo measurement information in a zero-speed detection module by utilizing information of an accelerometer, a gyroscope and a magnetometer, simultaneously carrying out initial magnetic heading error self-observation, zero-speed heading error self-observation in a dynamic state and geomagnetic matching, and constructing different Kalman filtering measurement equations when different conditions are met, wherein the state equations are consistent; compensating the position, the speed and the attitude obtained by the strapdown inertial navigation system by the position, the speed and the attitude error of the filtered estimation so as to obtain a final navigation positioning result, namely the position, the speed and the attitude of the pedestrian; returning the sensor error of the filtering estimation to an accelerometer and a gyroscope error compensation loop, and correcting the sensor error in real time so as to circulate;
when only zero-speed detection is successful, adopting pseudo measurement information to construct a correction model algorithm based on zero-speed Kalman filtering; when zero-speed detection is successful and initial magnetic heading error self-observation is carried out, a magnetic heading error self-observation algorithm in the pedestrian initial static state is established; when zero-speed detection is successful and zero-speed course error self-observation is carried out in a dynamic state, a zero-speed course error self-observation algorithm in a pedestrian motion state is established; when zero-speed detection is successful, and zero-speed heading error self-observation and geomagnetic matching are carried out in a motion state, a fusion algorithm based on geomagnetic matching and strapdown navigation resolving algorithms is established, wherein a Kalman filter correction model is established under the combined action of a state equation and a measurement equation of each algorithm.
The state equation of the Kalman filtering correction model is as follows:
the Kalman filtering model selects an' northeast China (ENU) geographic coordinate system, and an 18-order state model is constructed as follows:
wherein δ Φ is [ δ Φ ]e δφn δφu]Is the platform angle error; δ ν ═ δ ve δvn δvu]Is the speed error; δ p ═ δ λ δ L δ h]The position error is longitude, latitude and altitude error; epsilonb＝[εbx εby εbz]Is a triaxial gyro random constant; epsilonr＝[εrx εry εrz]Is a first order Markov process of a three axis gyroscope +r＝[▽rx ▽ry ▽rz]A first order Markov process for a three axis accelerometer;
under different coordinate systems
Moment of attitude transferThe arrays are different, A, G, W coefficients in the state equation are also different, and under the geographical coordinate system of northeast, A, G, W setting is as shown in the following four formulas: a is the system matrix, G is the system noise matrix, and W is [ omega ]gxωgy ωgz ωrx ωry ωrz ωax ωay ωaz]The system noise is set according to the characteristics of the sensor; wherein ω isgIs a random white noise drift of a triaxial gyro with a mean square error of sigmag，ωrThe mean square error of the three-axis gyroscope is sigmarIs driven by the Markov process ofaThe mean square error of the triaxial accelerometer is sigmaaThe markov process of (1) drives white noise;
the first algorithm is as follows: strapdown inertial navigation resolving algorithm
The inertial navigation system is an autonomous navigation system which does not depend on any external information and does not radiate energy to the outside, has the characteristics of good concealment and can work in various complex environments such as air, ground, underwater and the like, and is mainly divided into a platform type inertial navigation system and a strapdown type inertial navigation system. The strapdown inertial navigation system is developed on the basis of a platform type inertial navigation system, and is a frameless system.
The inertial sensor consists of an accelerometer and a gyroscope, the accelerometer and the gyroscope are directly and fixedly connected to the carrier, the gyroscope and the accelerometer are respectively used for measuring angular motion information and linear motion information of the carrier, and the onboard computer is used for calculating the course, the attitude, the speed and the position of the carrier according to the measurement information. The accelerometer acquires specific force information of a moving carrier, velocity can be acquired through primary integration, and position can be acquired through secondary integration; the angular velocity of the gyroscope system relative to the inertial system can be obtained through one-time integration. The most basic strapdown inertial navigation solution algorithm comprises attitude solution, speed solution and position solution. The attitude, velocity, position are then calculated from the following equations:
wherein b is a coordinate system of the body, n is a navigation coordinate system, e is a terrestrial coordinate system, i is an inertial coordinate system, f is a specific force, g is a gravitational acceleration,
is a posture transfer matrix from a body coordinate system to a navigation coordinate system,
is the projection of the x, y and z three-axis machine system on the machine system relative to the angular velocity of the inertial system,
for the projection of the three-axis navigation system on the machine system relative to the angular velocity of the inertial system,
is the projection of the angular rate of rotation of the earth under the navigation system,
is a column vector formed by the component of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the axial direction of the navigation coordinate system, v is a triaxial velocity vector, p is a triaxial position vector,
is composed of
With respect to the derivative of the time t,
is v isnWith respect to the derivative of the time t,
is pnThe derivative with respect to time t, and therefore the above equation, is an ordinary differential equation, which is solved iteratively on the basis of the knowledge of the initial attitude, velocity, position.
However, the low-cost inertial sensor has large noise, and can cause positioning errors of more than meter level within a few seconds, so that other additional auxiliary means are required to be searched for correcting navigation positioning errors, and error divergence is inhibited.
And (3) algorithm II: kalman filtering correction algorithm model based on zero speed
Constructing a Kalman filtering correction model based on zero speed, estimating a sensor error and a navigation positioning result error, and ensuring the positioning accuracy of the pedestrian at high and long time; when only zero-speed detection is successful, constructing a six-dimensional measurement model in a zero-speed state by using the pseudo measurement information; let veloP be a zero matrix of 3 × 1 order, posip (t) posiN (t-1), and the continuous equation of the kalman filter measurement model based on the zero velocity is shown as follows:
wherein Z is1(t) is the observed quantity of the speed and the position, veloN is the real-time resolving speed of the strapdown inertial navigation system, and posiN is the real-time resolving speed of the strapdown inertial navigation systemCalculated position, MvFor speed error of pedestrian at zero speed, MpFor the position error of the pedestrian at zero speed, V1For measuring noise, H1For the measurement matrix, the following two equations are shown:
Rmradius of curvature of the earth in the meridian, RnThe radius of curvature of the earth is expressed by the unitary-mortise circle, and L is latitude.
The position and speed virtual measurement information is used for assisting in correcting the sensor error and the navigation result error so as to maintain the pedestrian navigation positioning accuracy under high and long time, but in the second algorithm, the navigation observability is low, and the accuracy of the attitude can be directly influenced by the accuracy of the course, and the course error needs to be corrected by trying to construct course related measurement information.
And (3) algorithm III: magnetic heading error self-observation algorithm in pedestrian initial static state
The course accuracy in the pedestrian navigation positioning is a main factor influencing the pedestrian navigation positioning, and the magnetic course angle can be used as absolute course information to correct course errors. Under the initial static state of the pedestrian, the magnetic heading angle has stronger stability, so when zero-speed detection succeeds and the initial magnetic heading error is observed, a one-dimensional measurement equation is constructed, which is shown as the following formula:
wherein Z is2(t) is the observed quantity of the initial heading angle,. psiINSThe course angle resolved for strapdown can be calculated by the first algorithm
Matrix trigonometric function conversionObtain, order
Then
ψmgThe magnetic heading angle can be obtained by resolving magnetic information measured by a magnetic sensor,
resolving the error for the magnetic heading angle, V2To measure noise.
In the initial static state of the pedestrian, the sensor error is further estimated by the following formula:
Z3(t) observations of speed, position and initial heading angle.
And (4) algorithm four: zero-speed course error self-observation algorithm in pedestrian motion state
Under the moving state of the pedestrian, when each step goes through a section of zero-speed moment, the course is basically unchanged in the zero-speed stage, and based on the zero-speed course invariant theory, when zero-speed detection is successful and zero-speed course error self-observation is carried out in the dynamic state, the course angle of the first zero-speed time point of the step is recorded as psik-initialRecording the course angle of the rest zero-speed time points in the step as psik-others。
The one-dimensional zero-speed course self-observation measurement equation is constructed as follows:
wherein: z4(t) is the observed quantity of the heading angle in the motion state,
is the course angle error;
for zero speed course angle error, V4To measure noise; thus:
theta is a pitch angle and can be obtained by the algorithm I
The matrix is obtained by performing a trigonometric function conversion,
thus, the 7-dimensional measurement model added with the zero-speed heading error self-observation algorithm in the motion state is as follows:
wherein Z is5And (t) the observed quantity of the heading angle under the states of speed, position and motion.
And (5) algorithm five: fusion algorithm based on geomagnetic matching and strapdown navigation resolving algorithm
The geomagnetic matching information is utilized to increase the positioning reliability, and the pedestrian navigation positioning precision can be further guaranteed under the condition of geomagnetic matching. When zero-speed and geomagnetic matching are detected at the same time successfully, a 10-dimensional measurement equation is constructed by using a centralized filter to assist in correcting position errors, speed errors and sensor errors, as shown in the following three formulas, wherein posiM is a geomagnetic matching position and can be directly obtained by geomagnetic matching, and M is a geomagnetic matching position6For geomagnetic matching of position errors, V6For measuring noise, posiN is the position of inertial navigation real-time solution, δ p is the position error, and HI is shown in algorithm II.
Z6(t)＝[posiN-posiM]＝[δp+M6]＝H6(t)X(t)+V6(t)
H6＝[03×9 HI 03×9]
Z6(t) is geomagnetism matching position observed quantity, Z7And (t) the speed, the position, the heading angle in the motion state and the geomagnetic matching position observed quantity.
Therefore, a pedestrian high-precision foot navigation algorithm with multi-information fusion compensation is constructed, and high-precision and high-long-term navigation of pedestrians is guaranteed. The second, third, fourth and fifth algorithms actually utilize multi-source information to improve the accuracy and fault tolerance of pedestrian navigation positioning.
The algorithm is the most basic strapdown inertial navigation resolving algorithm, and for low-cost inertial sensors, the noise is large, and if the inertial sensors are not corrected, positioning errors above the meter level can be caused in a short time. Therefore, an algorithm II is adopted, and position and speed virtual measurement information is utilized to assist in correcting sensor errors and navigation result errors so as to maintain the navigation positioning accuracy of the pedestrians at high and long time, however, a Kalman filter in the algorithm II has low course observability, and the course is closely related to the attitude, and a course related measurement information needs to be constructed to correct the course errors; the magnetic heading angle can be used as absolute heading information to correct a heading error, and the magnetic heading angle has stronger stability in an initial static state of a pedestrian, so that the third algorithm is added, but the pedestrian does not necessarily have the initial static state, so that the fourth algorithm is added, the heading observability is improved, the heading error is reduced, and the positioning accuracy is further improved; and the fifth algorithm utilizes geomagnetic matching information to increase positioning reliability, and can further guarantee the navigation positioning precision of the pedestrian under the condition of geomagnetic matching.
The foregoing is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, various modifications and decorations can be made without departing from the principle of the present invention, and these modifications and decorations should also be regarded as the protection scope of the present invention.
Claims (1)
1. A pedestrian high-precision foot navigation method based on multi-information fusion compensation is characterized by comprising the following steps: acquiring original measurement data of a gyroscope and an accelerometer in real time, wherein the error compensation of the initial gyroscope is zero, the error compensation of the accelerometer is also zero, and strapdown calculation is carried out through a strapdown inertial navigation calculation algorithm; on the basis, a Kalman filter correction model is constructed for filtering correction, the state equations are consistent, the measurement equations are dynamically adjusted along with multidimensional measurement information, corresponding algorithms are constructed under different multidimensional information detection, the measurement equations are transformed in real time, and the errors of the navigation positioning result and the errors of the sensor are estimated and corrected, and the errors of the gyroscope and the accelerometer are returned; finally, outputting a navigation positioning result, namely the position, the speed and the posture of the pedestrian;
the strapdown inertial navigation resolving algorithm is as follows:
the inertial sensor consists of an accelerometer and a gyroscope, the accelerometer acquires specific force information of a motion carrier, velocity is acquired through primary integration, and position is acquired through secondary integration; the angular velocity of a gyroscope measuring machine system relative to an inertial system obtains an angle through primary integration; the most basic strapdown inertial navigation resolving algorithm comprises attitude resolving, speed resolving and position resolving; the attitude, velocity, position are then calculated from the following equations:
wherein b is a coordinate system of the body, n is a navigation coordinate system, e is a terrestrial coordinate system, i is an inertial coordinate system, f is a specific force, g is a gravitational acceleration,
is a posture transfer matrix from a body coordinate system to a navigation coordinate system,
is the projection of the x, y and z three-axis machine system on the machine system relative to the angular velocity of the inertial system,
for the projection of the three-axis navigation system on the machine system relative to the angular velocity of the inertial system,
is the projection of the angular rate of rotation of the earth under the navigation system,
the vector is a row vector formed by the components of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the axial direction of the navigation coordinate system, v is a triaxial velocity vector, and p is a triaxial position vector;
is composed of
With respect to the derivative of the time t,
is v isnWith respect to the derivative of the time t,
is pnThe derivative with respect to time t, therefore the above equation is an ordinary differential equation, and the solution is iterated on the basis of the known initial attitude, velocity and position;
the state equation of the Kalman filtering correction model is as follows:
the Kalman filtering model selects a geographical coordinate system of 'northeast sky', and an 18-order state model is constructed as follows:
wherein δ Φ is [ δ Φ ]e δφn δφu]Is the platform angle error; δ ν ═ δ ve δvn δvu]Is the speed error; δ p ═ δ λ δ L δ h]The position error is longitude, latitude and altitude error; epsilonb＝[εbx εby εbz]Is a triaxial gyro random constant; epsilonr＝[εrx εry εrz]For a three-axis gyroscope first order markov process,
a first order Markov process for a three axis accelerometer;
under different geographic coordinate systems
Attitude transition matrices are different, A, G, W coefficients in the state equation are also different, and under the geographical coordinate system of northeast, A, G, W setting is as shown in the following four formulas: a is the system matrix, G is the system noise matrix, and W is [ omega ]gxωgy ωgz ωrx ωry ωrz ωax ωay ωaz]The system noise is set according to the characteristics of the sensor; wherein ω isgIs a random white noise drift of a triaxial gyro with a mean square error of sigmag，ωrThe mean square error of the three-axis gyroscope is sigmarIs driven by the Markov process ofaThe mean square error of the triaxial accelerometer is sigmaaThe markov process of (1) drives white noise;
when only zero-speed detection is successful, adopting pseudo measurement information to construct a correction model algorithm based on zero-speed Kalman filtering; when zero-speed detection is successful and initial magnetic heading error self-observation is carried out, a magnetic heading error self-observation algorithm in the pedestrian initial static state is established; when zero-speed detection is successful and zero-speed course error self-observation is carried out in a dynamic state, a zero-speed course error self-observation algorithm in a pedestrian motion state is established; when zero-speed detection is successful, and zero-speed heading error self-observation and geomagnetic matching are carried out in a motion state, constructing a fusion algorithm based on geomagnetic matching and strapdown inertial navigation resolving algorithms, wherein a Kalman filter correction model is constructed under the combined action of a state equation and a measurement equation of each algorithm;
the correction model algorithm based on the zero-speed Kalman filtering is as follows:
when only zero-speed detection is successful, constructing a six-dimensional measurement model in a zero-speed state by using the pseudo measurement information; let veloP be a zero matrix of 3 × 1 order, posip (t) posiN (t-1), and the kalman filter measurement model based on zero velocity is as follows:
wherein Z is1(t) observed quantities of speed and position, veloN is the real-time resolving speed of the strapdown inertial navigation system, posiN is the real-time resolving position of the strapdown inertial navigation system, MvFor speed error of pedestrian at zero speed, MpFor the position error of the pedestrian at zero speed, V1For measuring noise, H1For the measurement matrix, the following two equations are shown:
Rmradius of curvature of the earth in the meridian, RnThe radius of curvature of the earth of the prime circle is L, and L is latitude;
the magnetic heading error self-observation algorithm in the pedestrian initial static state is as follows:
under the initial static state of the pedestrian, the magnetic heading angle has stronger stability, so when zero-speed detection succeeds and the initial magnetic heading error is observed, a one-dimensional measurement equation is constructed, which is shown as the following formula:
wherein Z is2(t) is the observed amount of the initial heading angle,
is the course angle error, psiINSCourse angle resolved for strapdown from strapdown inertial navigation solution algorithm
The matrix is obtained by trigonometric function conversion
Then
ψmgIs the magnetic heading angle and is obtained by resolving the magnetic information measured by the magnetic sensor,
resolving the error for the magnetic heading angle, V2To measure noise;
in the initial static state of the pedestrian, the sensor error is further estimated by the following formula:
Z3(t) observations of speed, position and initial heading angle;
the zero-speed course error self-observation algorithm in the pedestrian motion state is as follows:
under the moving state of the pedestrian, when each step goes through a section of zero-speed moment, the course is basically unchanged in the zero-speed stage, and based on the zero-speed course invariant theory, when zero-speed detection is successful and zero-speed course error self-observation is carried out in the dynamic state, the course angle of the first zero-speed time point of the step is recorded as psik-initialRecording the course angle of the rest zero-speed time points in the step as psik-others；
The one-dimensional zero-speed course self-observation measurement equation is constructed as follows:
wherein: z4(t) is the observed quantity of the heading angle in the motion state,
is the course angle error;
for zero speed course angle error, V4To measure noise; thus:
theta is a pitch angle and is calculated in a strapdown inertial navigation algorithm
The matrix is obtained by performing a trigonometric function conversion,
thus, the 7-dimensional measurement model added with the zero-speed heading error self-observation algorithm in the motion state is as follows:
wherein Z is5(t) the observed quantity of the course angle under the states of speed, position and motion;
the fusion algorithm based on the geomagnetic matching and the strapdown navigation resolving algorithm is as follows:
when zero-speed and geomagnetic matching are detected at the same time successfully, a 10-dimensional measurement equation is constructed by using a centralized filter to assist in correcting position errors, speed errors and sensor errors, as shown in the following three formulas, wherein posiM is a geomagnetic matching position and is directly obtained by geomagnetic matching, and M is a maximum value6For geomagnetic matching of position errors, V6For measuring noise, posiN is a position solved by inertial navigation in real time, δ p is a position error, and HI is shown in a correction model algorithm based on zero-speed Kalman filtering:
Z6(t) is geomagnetism matching position observed quantity, Z7And (t) the speed, the position, the heading angle in the motion state and the geomagnetic matching position observed quantity.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710716885.9A CN107655476B (en) | 2017-08-21 | 2017-08-21 | Pedestrian high-precision foot navigation method based on multi-information fusion compensation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710716885.9A CN107655476B (en) | 2017-08-21 | 2017-08-21 | Pedestrian high-precision foot navigation method based on multi-information fusion compensation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107655476A CN107655476A (en) | 2018-02-02 |
| CN107655476B true CN107655476B (en) | 2021-04-20 |
Family
ID=61127689
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710716885.9A Active CN107655476B (en) | 2017-08-21 | 2017-08-21 | Pedestrian high-precision foot navigation method based on multi-information fusion compensation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107655476B (en) |
Families Citing this family (35)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108627152B (en) * | 2018-04-25 | 2020-08-07 | 珠海全志科技股份有限公司 | Navigation method of micro unmanned aerial vehicle based on multi-sensor data fusion |
| CN109298436A (en) * | 2018-05-15 | 2019-02-01 | 重庆邮电大学 | A kind of indoor positioning and air navigation aid of multi-information fusion |
| CN109001787B (en) * | 2018-05-25 | 2022-10-21 | 北京大学深圳研究生院 | Attitude angle resolving and positioning method and fusion sensor thereof |
| CN108931155B (en) * | 2018-07-09 | 2020-08-14 | 北京航天控制仪器研究所 | Autonomous guidance system independent of satellite navigation extended-range guidance ammunition |
| CN108957510B (en) * | 2018-07-25 | 2022-05-24 | 南京航空航天大学 | Pedestrian seamless integrated navigation positioning method based on inertia/zero speed/GPS |
| CN109579836B (en) * | 2018-11-21 | 2022-09-06 | 阳光凯讯(北京)科技有限公司 | Indoor pedestrian orientation calibration method based on MEMS inertial navigation |
| CN109612464B (en) * | 2018-11-23 | 2022-06-10 | 南京航空航天大学 | Multi-algorithm enhanced indoor navigation system and method based on IEZ framework |
| CN109855620B (en) * | 2018-12-26 | 2020-10-09 | 北京壹氢科技有限公司 | Indoor pedestrian navigation method based on self-backtracking algorithm |
| CN109855621B (en) * | 2018-12-27 | 2023-06-02 | 国网江苏省电力有限公司检修分公司 | Combined indoor pedestrian navigation system and method based on UWB and SINS |
| CN109883429A (en) * | 2019-04-15 | 2019-06-14 | 山东建筑大学 | Zero-speed detection method and indoor pedestrian's inertial navigation system based on Hidden Markov Model |
| CN110763228B (en) * | 2019-10-08 | 2022-12-13 | 哈尔滨工程大学 | Error correction method of integrated navigation system based on seabed oil and gas pipe node position |
| CN110715659A (en) * | 2019-10-25 | 2020-01-21 | 高新兴物联科技有限公司 | Zero-speed detection method, pedestrian inertial navigation method, device and storage medium |
| CN110764506B (en) * | 2019-11-05 | 2022-10-11 | 广东博智林机器人有限公司 | Course angle fusion method and device of mobile robot and mobile robot |
| CN110736457A (en) * | 2019-11-12 | 2020-01-31 | 苏州工业职业技术学院 | combination navigation method based on Beidou, GPS and SINS |
| CN110763229B (en) * | 2019-11-12 | 2022-04-15 | 武汉大学 | Portable inertial navigation positioning rod and positioning and attitude determining method thereof |
| CN110793519A (en) * | 2019-11-26 | 2020-02-14 | 河南工业大学 | Incomplete measurement collaborative navigation positioning method |
| CN111024075B (en) * | 2019-12-26 | 2022-04-12 | 北京航天控制仪器研究所 | Pedestrian navigation error correction filtering method combining Bluetooth beacon and map |
| CN111189443B (en) * | 2020-01-14 | 2022-11-11 | 电子科技大学 | Pedestrian navigation method for online step length calibration, motion deviation angle correction and adaptive energy management |
| CN111197974B (en) * | 2020-01-15 | 2021-12-17 | 重庆邮电大学 | Barometer height measuring and calculating method based on Android inertial platform |
| CN111795694B (en) * | 2020-04-08 | 2022-05-10 | 应急管理部四川消防研究所 | Indoor positioning electromagnetic calibration method for fire rescue |
| CN111521178B (en) * | 2020-04-28 | 2021-01-15 | 中国人民解放军国防科技大学 | Drilling positioning director hole positioning method based on pipe length constraint |
| CN111750863A (en) * | 2020-06-18 | 2020-10-09 | 哈尔滨工程大学 | Navigation system error correction method based on auxiliary position of sea pipe node |
| CN111795696A (en) * | 2020-06-28 | 2020-10-20 | 中铁第一勘察设计院集团有限公司 | Initial structure optimization method of multi-inertial navigation redundancy system based on zero-speed correction |
| CN112033405B (en) * | 2020-08-10 | 2022-06-17 | 北京摩高科技有限公司 | Indoor environment magnetic anomaly real-time correction and navigation method and device |
| CN112362057B (en) * | 2020-10-26 | 2023-05-02 | 中国人民解放军海军航空大学 | Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation |
| CN112964257B (en) * | 2021-02-05 | 2022-10-25 | 南京航空航天大学 | Pedestrian inertia SLAM method based on virtual landmarks |
| CN112733409B (en) * | 2021-04-02 | 2021-11-30 | 中国电子科技集团公司信息科学研究院 | Multi-source sensing comprehensive integrated composite navigation micro-system collaborative design platform |
| CN113419265B (en) * | 2021-06-15 | 2023-04-14 | 湖南北斗微芯产业发展有限公司 | Positioning method and device based on multi-sensor fusion and electronic equipment |
| CN113608249B (en) * | 2021-07-16 | 2024-01-12 | 香港理工大学深圳研究院 | Indoor and outdoor seamless positioning and navigation library self-construction method |
| CN114001730B (en) * | 2021-09-24 | 2024-03-08 | 深圳元戎启行科技有限公司 | Fusion positioning method, fusion positioning device, computer equipment and storage medium |
| CN114061574B (en) * | 2021-11-20 | 2024-04-05 | 北京唯实深蓝科技有限公司 | Position-invariant constraint and zero-speed correction-based coal mining machine pose-determining and orienting method |
| CN114608570B (en) * | 2022-02-25 | 2023-06-30 | 电子科技大学 | Multi-mode self-switching pipeline instrument self-adaptive precise positioning method |
| CN114719858B (en) * | 2022-04-19 | 2024-05-07 | 东北大学秦皇岛分校 | 3-Dimensional positioning method based on IMU and floor height target compensation |
| CN117647251A (en) * | 2024-01-30 | 2024-03-05 | 山东科技大学 | Robust self-adaptive combined navigation method based on observed noise covariance matrix |
| CN117782001B (en) * | 2024-02-28 | 2024-05-07 | 爱瑞克(大连)安全技术集团有限公司 | PAPI navigation aid lamp dynamic angle monitoring and early warning method and system |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105628024A (en) * | 2015-12-29 | 2016-06-01 | 中国电子科技集团公司第二十六研究所 | Single person positioning navigator based on multi-sensor fusion and positioning and navigating method |
| CN105783923A (en) * | 2016-01-05 | 2016-07-20 | 山东科技大学 | Personnel positioning method based on RFID and MEMS inertial technologies |
| CN106705968A (en) * | 2016-12-09 | 2017-05-24 | 北京工业大学 | Indoor inertial navigation algorithm based on posture recognition and step length model |
-
2017
- 2017-08-21 CN CN201710716885.9A patent/CN107655476B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105628024A (en) * | 2015-12-29 | 2016-06-01 | 中国电子科技集团公司第二十六研究所 | Single person positioning navigator based on multi-sensor fusion and positioning and navigating method |
| CN105783923A (en) * | 2016-01-05 | 2016-07-20 | 山东科技大学 | Personnel positioning method based on RFID and MEMS inertial technologies |
| CN106705968A (en) * | 2016-12-09 | 2017-05-24 | 北京工业大学 | Indoor inertial navigation algorithm based on posture recognition and step length model |
Non-Patent Citations (2)
| Title |
|---|
| "Aiding MEMS IMU with Building Heading for Indoor Pedestrian Navigation";K.Abdulrahim;《2010 ubiquitous positioning indoor navigation and location based service》;20101231;正文第1-6页 * |
| 基于地磁辅助的室内行人定位航向校正方法;马明等;《电子与信息学报》;20170331(第03期);正文第647-653页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107655476A (en) | 2018-02-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
| CN109556632B (en) | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering | |
| Ludwig et al. | Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data | |
| CN109459044B (en) | GNSS dual-antenna assisted vehicle-mounted MEMS inertial navigation combined navigation method | |
| CN112629538B (en) | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering | |
| CN107588769B (en) | Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method | |
| CN102169184B (en) | Method and device for measuring installation misalignment angle of double-antenna GPS (Global Position System) in integrated navigation system | |
| CN104698485B (en) | Integrated navigation system and air navigation aid based on BD, GPS and MEMS | |
| CN109916395B (en) | Gesture autonomous redundant combined navigation algorithm | |
| CN104697520B (en) | Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method | |
| CN201955092U (en) | Platform type inertial navigation device based on geomagnetic assistance | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| CN112432642B (en) | Gravity beacon and inertial navigation fusion positioning method and system | |
| CN111024070A (en) | Inertial foot binding type pedestrian positioning method based on course self-observation | |
| CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
| CN109612460B (en) | Plumb line deviation measuring method based on static correction | |
| CN111121766A (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN102879779A (en) | Rod arm measurement and compensation method based on synthetic aperture radar (SAR) remote sensing imaging | |
| CN103278165A (en) | Remanence-calibration-based autonomous navigation method of magnetic survey and starlight backup based on | |
| CN108151765B (en) | Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error | |
| CN112880669A (en) | Spacecraft starlight refraction and uniaxial rotation modulation inertia combined navigation method | |
| CN117053782A (en) | Combined navigation method for amphibious robot | |
| CN105928519B (en) | Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer | |
| CN110095117A (en) | A kind of air navigation aid that gyro free inertia measurement system is combined with GPS | |
| CN108416387B (en) | Height filtering method based on fusion data of GPS and barometer |
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 |