CN110553646B - Pedestrian navigation method based on inertia, magnetic heading and zero-speed correction - Google Patents

Abstract

The invention discloses a pedestrian navigation method based on inertial and magnetic heading and zero-speed correction, including: (1) using inertial measurement elements to measure linear motion information and angular motion information of pedestrians, and obtaining the attitude, speed and position of pedestrians through calculation information; use the three-axis magnetic component measured by the magnetometer to solve the pedestrian's heading information; (2) through the zero-speed interval detection algorithm, get the zero-speed state during the walking process, and update the observation of the speed error in this state , combined with the observation of the heading angle error, the Kalman filter equation is established to correct the error of the pedestrian navigation output, and finally complete the output of the corresponding navigation information. Therefore, the present invention uses the three-axis magnetic induction data to calculate the magnetic heading angle to make up for the deficiencies of the inertial navigation system. In this process, the measurement data of the inertial measurement unit is used to judge the zero-speed interval, and the system error model and Kalman filter equation are established, and the appropriate observation quantity is selected to estimate the state error.

Description

Pedestrian Navigation Method Based on Inertial and Magnetic Heading and Zero Speed Correction

technical field

The invention relates to a pedestrian navigation method based on inertial and magnetic heading and zero-speed correction.

Background technique

Traditional pedestrian navigation technology is mostly based on satellite positioning, such as GPS or Beidou. But in some special occasions, such as when a forest fire breaks out in a deep mountain, the shielding of the mountain often makes the satellite navigation system unable to work. In addition, satellite navigation systems often fail when people are walking on high-rise streets or indoors. In real life, some special fields need to locate personnel quickly and accurately even when satellite positioning signals fail. In these cases, how to obtain accurate pedestrian navigation/location information is very important. The principle of inertial navigation is based on Newton's classical mechanics. It uses the angular motion and linear motion information of the carrier provided by inertial measurement elements (such as gyroscopes and accelerometers), and finally obtains the attitude, velocity and position of the carrier through coordinate transformation and algorithm calculation. The advantage of inertial navigation is that it has complete autonomy and is not affected by the external environment. There is no problem that the satellite positioning system is unavailable in special environments. The disadvantage of the inertial navigation system is that there are measurement errors in the inertial components themselves, such as drift errors in the gyroscope, and the inertial navigation system also has its own errors. Therefore, the pure strapdown inertial navigation system can only maintain the positioning requirements for a period of time. When working for a long time, the navigation error diverges rapidly and cannot provide effective services. This problem is especially acute in low-cost inertial navigation systems. Magnetic heading aided navigation technology is a navigation technology that provides the heading angle information of the carrier based on geomagnetic data. Due to the existence of the geomagnetic field, the geomagnetic field strength vector at any point on the earth corresponds to its longitude and latitude one by one, so it can avoid the problem of accumulative heading angle error in the inertial navigation system. However, since the magnetic heading aided navigation needs to measure the magnetic field of the surrounding environment of the carrier, the change of the electromagnetic characteristics of the external environment has a certain influence on the accuracy of geomagnetic navigation.

The pure inertial navigation system or magnetic heading assisted navigation technology has some shortcomings, which cannot meet the high-precision positioning requirements of the navigation system under long-term working conditions. In order to improve the performance of the pedestrian navigation system, a multi-sensor information fusion integrated navigation system is established based on micro-inertial navigation and zero-speed correction technology, combined with magnetic heading aided navigation. Use different navigation methods to complement each other, correct the cumulative error of the inertial navigation system, and make each navigation technology play its own characteristics and strengths.

The principle of zero-speed correction technology is that there is a very short time interval when the instantaneous speed is zero during each walking process of pedestrians. We call this interval the zero-speed interval. During this time interval, the soles of the feet touch the ground, the soles of the load-bearing legs are in full contact with the ground, and the true value of the linear velocity of the human body should be zero. However, due to error accumulation and other reasons, the speed value actually output by the navigation system is generally not zero, and the speed error can be obtained by subtracting the two values. The Kalman filter equation is established according to the error model of the inertial navigation system, and the error quantity is taken as one of the observations, so that the estimation of the state quantity can be updated in each zero-speed interval and the navigation error can be corrected in real time. The zero-speed correction technology can effectively reduce the position error, velocity error and horizontal attitude error, but it cannot effectively correct the heading angle error.

The Chinese invention patent with the announcement number CN108362282A published on August 3, 2018 "A Method for Inertial Pedestrian Positioning Based on Adaptive Zero-speed Interval Adjustment". The sliding window detection method adaptively adjusts the length of the zero-speed detection window during the system solution process, improves the accuracy of zero-speed detection, enhances the applicability of the inertial pedestrian positioning system at any speed, and reduces the impact caused by changes in pedestrian speed. The resulting error compensation is insufficient.

Journal of Tsinghua University (Natural Science Edition), Volume 56, Issue 2, 2016, "On-line calibration of three-axis magnetometer for pedestrian navigation based on foot-mounted INS" written by Zhang Xinxi, Zhang Rong et al. The magnetometer error model and online calibration algorithm. According to the error characteristics of the magnetometer and the mobility of the foot-mounted INS, the state equation and the observation equation of the magnetometer error variable are established, and the extended Kalman filter (EKF) is used to estimate and analyze the three-axis magnetometer error online. Real-time calibration, using the zero-speed correction algorithm and the magnetic heading angle constraint algorithm to constrain the error of the foot-mounted INS. The experiment proves that the algorithm realizes the online calibration of pedestrian navigation magnetometer error based on the foot-mounted INS, and greatly improves the positioning accuracy of pedestrian autonomous navigation.

Chinese Journal of Inertial Technology, Volume 20, Issue 5, 2012, "Personal Navigation Method Based on Foot Micro-inertia/Geomagnetic Measurement Components" written by Qian Weixing, Zhu Xinhua, etc. This article studies a foot-mounted micro-inertia A personal navigation positioning method for a geomagnetic measurement component. The method uses the information of the micro-inertial measurement components to carry out strapdown inertial navigation solution to obtain the attitude, speed and position information of the human foot, uses the magnetic sensor to determine the heading information of the movement, and adopts the zero-speed correction method based on the gait phase detection, real-time Correct the navigation information error of the MEMS inertial navigation system and the random error of the inertial sensor, thereby slowing down the accumulation of the positioning error of the inertial navigation system over time. The experimental results prove that the proposed method can effectively improve the positioning accuracy of the personal navigation system, and it can realize the personal navigation positioning for a long time in the environment of GNSS signal attenuation or failure.

Chinese Journal of Inertial Technology, Volume 24, Issue 1, 2016, "Multi-Constrained Pedestrian Navigation Zero-Speed Interval Detection Algorithm" written by Tian Xiaochun, Chen Jiabin and others, this article designed a A pedestrian gait zero-speed interval detection algorithm with multiple constraints. This method comprehensively utilizes the modulus, variance, amplitude, and peak value of the output parameters of the foot sensor and extracts the zero-speed point in the gait by setting the threshold. The multi-condition constraints effectively reduce the possibility of misjudgment. Finally, this method is used to detect the zero-speed intervals in the process of walking at different synchronous speeds. The results show that the number of zero-speed intervals calculated by the multi-condition constraint method is completely consistent with the number of zero-speed intervals in the actual gait.

The above literatures all use the error model of the inertial navigation system to establish the system equations, and select the appropriate observations to estimate the error and correct the positioning data of the navigation system in real time, but they are not comprehensive enough. The reason is that the application of Kalman filter to optimally estimate the navigation error must also give the corresponding system state equation and the specific form of the measurement equation according to the difference between the selected state quantity and observation quantity. The dimension and structure of the Kalman filter; the Kalman filter must know the calculation method of the system state transition matrix F matrix and the observation matrix H matrix before it can be iterated. However, the current published literature does not give the specific forms of the system state equation and measurement equation of the pedestrian positioning system, nor does it give the state transition matrix F matrix and observation matrix H matrix in the equation. These need to be selected according to the system error model and The state quantity of the system can only be obtained after careful design and calculation, which is the most core part of the zero-speed correction technology.

Contents of the invention

Aiming at the deficiencies of the prior art, the invention provides a pedestrian navigation method based on inertial and magnetic heading and zero-speed correction.

Specifically include the following technical solutions:

A pedestrian navigation method based on inertial and magnetic heading and zero speed correction, comprising the following steps:

(1) Use inertial measurement elements to measure the linear motion information and angular motion information of pedestrians, and solve through coordinate transformation and strapdown inertial navigation algorithm to obtain pedestrian attitude, speed and position information;

Using the three-axis magnetic component measured by the magnetometer, the pedestrian's heading information is obtained through coordinate transformation and strapdown inertial navigation algorithm solution;

(2) Through the zero-speed interval detection algorithm, the zero-speed state in the walking process is obtained, and the observation of the speed error is updated in this state, combined with the observation of the heading angle error, the Kalman filter equation is established to correct the error of the pedestrian navigation system , and finally complete the output of the corresponding navigation positioning information.

Further, the state equation of the Kalman filter equation is as follows:

In the formula, X(t) is the system state quantity, F is the system state transition matrix, W(t) is the system noise; the specific form of the above formula is:

in:

ω _ie is the angular velocity of the earth's rotation, L is the latitude value, R is the radius of curvature of the earth; g is the acceleration of gravity;

δγ, δθ, δψ are roll angle error, pitch angle error and heading angle error in turn;

δv _x , δv _y are eastward speed error and northward speed error respectively; δL is latitude error;

分别是x轴和y轴的加速度计随机误差；ε_x，ε_y，ε_z分别是x轴、y轴和z轴的陀螺仪随机误差；

are the accelerometer random errors of the x-axis and y-axis respectively; ε _x , ε _y , ε _z are the gyroscope random errors of the x-axis, y-axis and z-axis respectively;

Assuming that the first-order Markov process ε _r and the random constant ε _b constitute the random drift model of the gyroscope, then:

ε=ε _b +ε _r ,

where ε is the measurement random error of the gyroscope, and β is the anticorrelation time constant of the first-order Markov process,

w ₁ is random noise;

构成了加速度计的随机误差模型，即Assuming a first-order Markov process

constitutes the random error model of the accelerometer, namely

是加速度计的测量随机误差，μ是反相关时间常数，w₂为随机噪声；In the formula

is the measurement random error of the accelerometer, μ is the anti-correlation time constant, w ₂ is the random noise;

The observed values select the observed values of eastward and northward speed errors and the observed values of heading angle errors, and the observation equation is:

Z(t)=HX(t)+N(t)

In the formula: Z(t) is the system observation quantity, H is the system observation matrix, N(t) is the system observation noise; the specific form of the above formula is:

是水平速度误差的观测量，由零速状态下捷联式惯性导航算法输出的水平速度v_x、v_y求得；

是航向角误差的观测量，由磁力计的输出与捷联式惯性导航算法解算出的航向角的差值求得；N_Vyaw是水平速度误差的观测噪声，N_ψ是航向角误差的观测噪声；它们可以认为是小的白噪声。In the formula

is the observed quantity of the horizontal velocity error, obtained from the horizontal velocity v _x and v _y output by the strapdown inertial navigation algorithm in the zero-speed state;

is the observed value of the heading angle error, obtained from the difference between the output of the magnetometer and the heading angle calculated by the strapdown inertial navigation algorithm; N _Vyaw is the observation noise of the horizontal velocity error, and N _ψ is the observation noise of the heading angle error ; they can be considered as small white noise.

After the system state equation and observation equation are established, the Kalman filter can be iterated to optimally estimate the system state quantity to obtain the navigation error and correct it.

是否表明当前脚部运动状态处于零速区间之中，具体如下：Further, the zero-speed interval detection algorithm in step (2) is: when the three-axis acceleration vector sum |A(t _K )| output by the inertial measurement element at time t _k indicates that the current foot movement state is in the zero-speed process, then Judging the acceleration vector and variance at time t _k

Whether it indicates that the current foot movement state is in the zero-speed range, as follows:

2-1. The three-axis acceleration vector sum |A(t _K )| output by the inertial measurement element at time t _k satisfies:

Among them: k=1,2,...,n,n+1; a _x (t _k ), a _y (t _k ), a _z (t _k ) respectively refer to the X, Y output by the inertial measurement element at the time t _k , the component of the Z three-axis;

The judgment formula of the three-axis acceleration vector sum |A(t _K )| in the process of zero speed is:

th _a is the judgment threshold of the three-axis acceleration vector and |A(t _k )|, g is the acceleration of gravity;

满足：2-2. Acceleration vector and variance at time t _k

satisfy:

为采样区间内的加速度向量和的平均值；In the formula: n is the width of the sampling interval, which is selected according to the output characteristics of the inertial element,

is the average value of the acceleration vector sum in the sampling interval;

Judgment formula of zero-speed process variance:

小于设定的方差阈值th_b时，判断表达式输出为1，判定人体处于零速状态中。In the formula: th _b is the judgment threshold of the acceleration vector and variance at time t _k ; when the acceleration vector and variance at time t _k

When it is smaller than the set variance threshold th _b , the output of the judgment expression is 1, and it is determined that the human body is in a zero-speed state.

Further, the value of the judgment threshold th _a of the three-axis acceleration vector sum |A(t _k )| is 0.46, and the value of the variance threshold th _b is 0.28.

Further, the observed quantity of heading angle error satisfies:

where ψ _Ins is the heading angle calculated by the strapdown inertial navigation algorithm, and Ψ _r is the heading angle output by the magnetometer after magnetic declination correction;

ψ _r =ψ _m -ψ ₀

Ψ ₀ is the magnetic declination, Ψ _m is the magnetic heading angle calculated from the magnetic component measured by the three-axis magnetometer, specifically:

X _h ＝M _bx cosθ+M _by sinγsinθ+M _bz cosγsinθ

Y _h ＝M _by cosγ-M _bz sinγ

为磁力计在行人坐标系的三轴磁分量，θ、γ分别指俯仰角和滚转角。

are the three-axis magnetic components of the magnetometer in the pedestrian coordinate system, and θ and γ refer to the pitch angle and roll angle, respectively.

The pedestrian navigation method based on inertial and magnetic heading and zero-speed correction according to claim 1, wherein the inertial measurement element includes an accelerometer for measuring linear motion information and a gyroscope for measuring angular motion information.

According to above-mentioned technical scheme, with respect to prior art, the present invention will have following beneficial effect:

1. The present invention first adopts a strapdown inertial navigation algorithm to carry out strapdown attitude, velocity and position calculations on the linear motion information and angular motion information measured by the inertial measurement element, to obtain the current attitude, velocity and position information, and at the same time through the magnetic force The heading information is obtained by calculating the three-axis magnetic component obtained by the meter measurement. Through the zero-speed interval detection algorithm, the zero-speed state in the walking process is obtained. In this state, the speed error is updated, combined with the heading angle error, the Kalman filter equation is established to correct the error of the pedestrian navigation system, and finally the output of the corresponding positioning and navigation information is completed. .

2. The present invention especially proposes the specific form of the system equation, which overcomes the state transition matrix F array and the observation matrix H array in the equation in the prior art, so that the Kalman filter equation based on zero-speed correction can be used for Optimal estimation of navigation error.

Description of drawings

Fig. 1 is a schematic diagram of the navigation solution of the pedestrian navigation system of the present invention.

Fig. 2 is a plane displacement diagram of an outdoor walking experiment of the pedestrian navigation system.

Fig. 3 is a plane displacement diagram of the indoor walking experiment of the pedestrian navigation system.

Detailed ways

The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some, not all, embodiments of the present invention. The following description of at least one exemplary embodiment is merely illustrative in nature and in no way taken as limiting the invention, its application or uses. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without creative efforts fall within the protection scope of the present invention. The relative arrangements, expressions and numerical values of components and steps set forth in these embodiments do not limit the scope of the present invention unless specifically stated otherwise. At the same time, it should be understood that, for the convenience of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship. Techniques, methods and devices known to those of ordinary skill in the relevant art may not be discussed in detail, but where appropriate, such techniques, methods and devices should be considered part of the Authorized Specification. In all examples shown and discussed herein, any specific values should be construed as illustrative only, and not as limiting. Therefore, other examples of the exemplary embodiment may have different values.

The pedestrian navigation method described in the present invention, as shown in FIG. 1 , has a core of SINS (Strapdown Inertial Navigation System, strapdown inertial navigation system) and a Kalman filter. Specifically, it includes: measuring linear motion information and angular motion information through inertial measurement elements, obtaining current attitude, speed and position information through calculation based on navigation algorithms, and obtaining heading information through calculation of three-axis magnetic components measured by magnetometers. Through the zero-speed interval detection algorithm, the zero-speed state in the walking process is obtained. In this state, the speed error is updated, combined with the heading angle error, the Kalman equation is established to correct the error of the pedestrian navigation system, and finally the output of the corresponding positioning and navigation information is completed.

1. Calculation of magnetic heading angle and heading angle error

θ、γ分别指惯性测量元件测量得到的俯仰角和滚转角。Suppose the magnetic components of the three-axis magnetometer in the pedestrian coordinate system are

θ and γ refer to the pitch angle and roll angle measured by the inertial measurement unit, respectively.

Then the X-axis and Y-axis components of the magnetometer in the navigation coordinate system can be expressed by the following formulas:

X _h ＝M _bx cosθ+M _by sinγsinθ+M _bz cosγsinθ

Y _h ＝M _by cosγ-M _bz sinγ

Then, the calculation formula of the magnetic heading angle can be obtained:

Although the magnetic heading angle can be obtained by the magnetometer, due to the existence of the magnetic declination Ψ ₀ , the actual heading angle output by the magnetometer is:

ψ _r =ψ _m -ψ ₀

Then, the observed value of the heading angle error can be calculated by the following formula, where ψ _Ins is the heading angle obtained by the inertial navigation system solution:

2. Strapdown inertial navigation update algorithm

In this design, a two-sample rotation vector algorithm is introduced to update attitude and velocity.

(1) Attitude update algorithm

为t_k时刻机体系对导航系的姿态四元数；

为t_k-1时刻机体系对导航系的姿态四元数；

为机体系由t_k时刻到t_k-1时刻转动对应的姿态四元数。

是机体系对导航系的转动角速率在机体系上的投影。Δφ为由t_k时到t_k-1时刻机体转动对应的旋转矢量；Δθ为机体由t_k时到t_k-1时刻对应的角增量。Δθ₁和Δθ₂是在两个相等的时间间隔内，对陀螺仪输出的角运动信息进行采样，得到的测量值，构成了二子样旋转矢量计算法。In the formula, b represents the aircraft system, n represents the navigation system, and t is the integration time.

is the attitude quaternion of the machine system to the navigation system at time t _k ;

is the attitude quaternion of the machine system to the navigation system at time t _k-1 ;

is the attitude quaternion corresponding to the rotation of the aircraft system from time t _k to time t _k-1 .

is the projection of the aircraft system on the aircraft system to the rotational angular velocity of the navigation system. Δφ is the rotation vector corresponding to the rotation of the body from time t _k to time t _k-1 ; Δθ is the corresponding angular increment of the body from time t _k to time t _k-1 . Δθ ₁ and Δθ ₂ are to sample the angular motion information output by the gyroscope in two equal time intervals, and the measured values obtained constitute the two-sample rotation vector calculation method.

(2) Speed update algorithm

V(t _k )＝V(t _k-1 )+C(t _k-1 )ΔV _sfm (t _k )+ΔV _g/corm (t _k )

为t_k-1时刻导航系对惯性系的角速度在导航系上的投影；

为t_k-1时刻导航系对地理系的角速度在导航系上的投影；C(t_k-1)为上一更新时刻的姿态矩阵，ΔV_sfm(t_k)为比力引发的速度补偿量，

为有害加速度引发的速度补偿量，ΔV_rotm(t_k)为旋转效应补偿项，ΔV_sculm(t_k)为划桨效应补偿项。ΔV₁和ΔV₂是在两个相等的时间间隔内，对加速度计输出的线运动信息进行采样，得到的测量值，构成了二子样。Ts是捷联惯导算法更新周期。Among them, e represents the geographic system, i represents the inertial system, V(t _k ) is the velocity of the body at the time t _k ; g ⁿ (t _k-1 ) is the gravitational acceleration at the time t _k-1 ;

is the projection of the angular velocity of the navigation system to the inertial system on the navigation system at time t _k-1 ;

is the projection of the angular velocity of the navigation system to the geographic system on the navigation system at time t _k-1 ; C(t _k-1 ) is the attitude matrix at the last update time, and ΔV _sfm (t _k ) is the velocity compensation caused by the specific force ,

is the speed compensation amount caused by harmful acceleration, ΔV _rotm (t _k ) is the rotation effect compensation item, and ΔV _sculm (t _k ) is the paddle effect compensation item. ΔV ₁ and ΔV ₂ are measured values obtained by sampling the linear motion information output by the accelerometer within two equal time intervals, constituting two sub-samples. Ts is the update cycle of the SINS algorithm.

(3) Location update algorithm

Because the height changes little when walking on flat ground, and the strapdown inertial navigation height calculation channel is unstable. Therefore, the position update of the strapdown inertial navigation system selects E and N (east and north) respectively, and measures the displacement in the two directions.

是T_k时刻机体系下行人的东向速度，

是T_k时刻机体系下行人的北向速度。Ts是捷联惯导算法的更新时间。行人导航系统在启动的时候有初始对准的过程，可以得到导航初始位置的基本地理信息。根据这些初始地理信息，最后就可以将行人坐标系下的位置转化为地球坐标系下的位置，这里不再细述。In the formula, E _k is the eastward position of the carrier at T _k time, N _k is the northward position of the carrier at T _k time,

is the eastward speed of pedestrians under the T _k time machine system,

is the northward velocity of pedestrians under the T _k time machine system. Ts is the update time of the strapdown inertial navigation algorithm. The pedestrian navigation system has an initial alignment process when it is started, and the basic geographic information of the initial position of the navigation can be obtained. According to these initial geographic information, the position in the pedestrian coordinate system can be transformed into the position in the earth coordinate system, which will not be described in detail here.

3. Strapdown inertial navigation error equation

(1) Attitude error

而δα,δβ,δλ分别近似等于横滚，俯仰和航向的欧拉角误差δγ,δθ,δψ。

是机体系对导航系的方向余弦阵。

是机体系对惯性系的转动角速率在机体系上的投影，

是其误差。

是导航系对惯性系的转动角速率在导航系上的投影，

是其误差。In the formula,

And δα, δβ, δλ are approximately equal to the Euler angle errors δγ, δθ, δψ of roll, pitch and heading respectively.

is the direction cosine matrix of the aircraft system to the navigation system.

is the projection of the rotational angular velocity of the machine system to the inertial system on the machine system,

is its error.

is the projection of the rotational angular rate of the navigation system to the inertial system on the navigation system,

is its error.

(2) Speed error

是速度误差的一阶导数。fⁿ是加速度计输出的比力f在导航系上的投影，f^b是加速度计输出的比力f在机体系上的投影，δf^b是f^b的误差。δg是重力加速度误差。

是地理系对惯性系的旋转角速度在地理系上的投影，

是其误差。

是导航系对地理系的旋转角速度在导航系上的投影，

是其误差。In the formula, v ⁿ is the velocity of the vehicle under the navigation system, which is what we generally call the velocity of the vehicle.

is the first derivative of the velocity error. f ⁿ is the projection of the specific force f output by the accelerometer on the navigation system, f ^b is the projection of the specific force f output by the accelerometer on the aircraft system, and δf ^b is the error of f ^b . δg is the gravitational acceleration error.

is the projection of the geographic system to the rotational angular velocity of the inertial system on the geographic system,

is its error.

is the projection of the rotation angular velocity of the navigation system to the geographic system on the navigation system,

is its error.

(3) Position error

4. Calculation of height information

Since the altitude calculation channel of the pure inertial navigation system is an unstable system, the pure strapdown inertial navigation system cannot be used to calculate the altitude of the carrier, but external altitude information such as barometric altimeter and radio altimeter must be introduced. In the present invention, the air pressure information obtained by the barometer is directly used to convert the local altitude, which is a mature content and will not be described in detail here.

5. Zero speed detection algorithm

When the foot is at zero speed, its three-axis acceleration vector sum should theoretically be a constant acceleration of gravity, but it will fluctuate in a small range near the constant acceleration of gravity during the actual measurement process. Select the corresponding threshold value according to the precision of the inertial element and the measurement data.

The above formula |A(t _k )| is the sum of three-axis acceleration vectors output by the inertial measurement element at time t _k (k=1, 2, . . . , n, n+1). a _x (t _k ), a _y (t _k ), a _z (t _k ) respectively refer to the acceleration components of the X, Y, and Z axes output by the inertial measurement element at the time t _k ;

The above formula is the first zero-speed state judgment expression, where g is the acceleration of gravity, th _a is the vector and the judgment threshold. The threshold is set to 0.46 by measurement experiments. When it is within the threshold range, it will output 1, indicating that it may be in a zero-speed state. On the contrary, outputting 0 means that it is not a zero-speed state.

Judging the zero-speed interval by the above method alone is not complete enough in practice. In order to improve the accuracy, it is necessary to introduce the judgment of variance statistics. After knowing the specific degree of dispersion within the zero-speed interval and the output characteristics of the accelerometer, by setting the corresponding window length and variance threshold, the variance of the acceleration values in the continuous scanning window is calculated. When the variance value is less than the set threshold, we It can be judged that the current foot movement state may be in the zero-speed range.

代表t_k时刻加速度向量和的方差，|A(t_k)|为前文计算的加速度向量和，n为采样数量，根据惯性元件的输出特性进行选择，

为采样区间内的加速度向量和的平均值。

Represents the variance of the acceleration vector sum at time t _k , |A(t _k )| is the acceleration vector sum calculated above, n is the number of samples, selected according to the output characteristics of the inertial element,

is the average value of the acceleration vector sum in the sampling interval.

The above formula is the second zero-speed state judgment expression, where th _b is the variance judgment threshold, and the threshold is set to 0.28 through measurement experiments.

In actual operation, the human body is considered to be in the zero-speed state only when the outputs of the first and second zero-speed state judgment expressions are both 1.

6. Kalman filter equation based on zero-speed correction

The Kalman filter equation is established by using the strapdown inertial navigation error equation.

依次为横滚角误差、俯仰角误差和航向角误差。

The order is roll angle error, pitch angle error and heading angle error.

δv ⁿ ＝[δv _x δv _y δv _z ], which are eastward velocity error, northward velocity error, and skyward velocity error in turn.

δp ⁿ ＝[δL δλ δh], followed by latitude error, longitude error, height error.

The complete inertial navigation error state variable contains 9 parameters, but the following points need to be noted:

1. The inertial navigation system uses east, north, and sky as the three-axis pointing;

2. δλ exists independently, so it is not put into the error state variable;

3. Considering the instability of the sky channel, the error state quantity of the channel is discarded, assuming δV _z =0;

After considering the above conditions, the state equation of the system is written as follows:

In the formula, X(t) is the system state quantity, F is the system state transition matrix, and W(t) is the system noise. Specifically:

In the formula:

ω _ie is the angular velocity of the earth's rotation, L is the latitude value, R is the radius of curvature of the earth; g is the acceleration of gravity;

δγ, δθ, δψ are roll angle error, pitch angle error and heading angle error in turn;

δv _x , δv _y are eastward speed error and northward speed error respectively; δL is latitude error;

分别是x轴和y轴的加速度计随机误差；ε_x，ε_y，ε_z分别是x轴、y轴和z轴的陀螺仪随机误差；假设一阶马尔科夫过程和随机常数构成了陀螺仪的随机漂移模型，即

are the accelerometer random errors of the x-axis and y-axis respectively; ε _x , ε _y , ε _z are the gyroscope random errors of the x-axis, y-axis and z-axis respectively; assume that the first-order Markov process and the random constant constitute the gyroscope The random drift model of the instrument, namely

Assuming that a first-order Markov process constitutes the stochastic error model of the accelerometer, that is

β and μ in the two formulas are anticorrelation time constants, w ₁ and w ₂ are random noises.

The observed values are the observed values of eastward and northward speed errors and the observed values of heading angle errors, and the system observation equation is:

Z(t)=HX(t)+N(t)

In the formula, Z(t) is the system observation quantity, H is the system observation matrix, and N(t) is the system observation noise. Specifically:

是水平速度误差的观测量，它可由零速状态下惯导输出的水平速度v_x,v_y求得。

是航向角误差的观测量，前已所述它可由磁力计的输出与惯导航向角输出的差值求得。N_Vyaw是水平速度误差的观测噪声，N_ψ是航向角误差的观测噪声。它们可以认为是小的白噪声。In the formula

is the observed quantity of the horizontal velocity error, which can be obtained from the horizontal velocity v _x , v _y output by the inertial navigation in the zero-speed state.

is the observed value of the heading angle error, which can be obtained from the difference between the output of the magnetometer and the output of the inertial heading angle as mentioned above. N _Vyaw is the observation noise of the horizontal velocity error, and N _ψ is the observation noise of the heading angle error. They can be thought of as small white noise.

After the system state equation and observation equation are established, the Kalman filter iteration can be carried out, so as to optimally estimate the system state quantity, estimate the navigation error, and then correct it. Since the speed error in the observation is only updated at the zero-speed moment, the Kalman filter equation designed in this paper has been updating time. Only when a new error value is obtained when it is determined to be in the zero-speed interval will the measurement update be performed.

Kalman filtering consists of 5 equations, which are classic and mature. For the sake of completeness and clarity in this article, the Kalman filter equation is attached here, but the explanation is omitted.

可以运用下列方式获得：The best estimate for X _k

It can be obtained in the following ways:

State one-step prediction

state valuation

filter gain

one-step forecast error variance

estimated error variance

This equation can be further written as

P _k ＝[IK _k H _k ]P _k,k-1

or

我们就可以利用这些误差的最优估计修正导航参数。例如：By iterating the Kalman filter, the optimal estimate of the system state quantity X(k) can be obtained. That is, the optimal estimation of roll angle error, pitch angle error, heading angle error, east speed error, north speed error, latitude error, etc.

We can use the best estimate of these errors to correct the navigation parameters. For example:

为修正后的横滚角。In the formula, "-" represents the revised navigation parameters, for example

is the corrected roll angle.

Using zero-speed detection and Kalman filtering to obtain error estimates and correct navigation parameters, these parameters can be substituted back into the strapdown inertial navigation solution equation to start the next round of navigation solution, which will not be described in detail here.

Example 1

Apply the above-mentioned pedestrian navigation method to the outdoor test, as follows:

As shown in Figure 2, it is the plane displacement diagram of the outdoor walking experiment of the pedestrian navigation system, in which the thin black line represents the displacement curve of the pedestrian navigation system using zero-speed correction and magnetic heading assistance.

The outdoor experiment site is the new playground of our school. In a clockwise direction, along the innermost white line of the 400m runway, from the starting point shown in the picture, walk around the playground clockwise at a constant speed, and finally return to the starting position to complete the entire experiment The travel time is about 5 minutes and 37 seconds. Before starting to walk, first stand still at the starting position for 30 seconds to complete the initial alignment. Because the start position and end position of the experiment are the same, the positioning error can be calculated based on the distance between the actual solution end position and the start position.

In the early stage of the journey, the displacement error of the pedestrian navigation system using zero speed correction is small. As time goes by, the inertial navigation error accumulates, and the position data obtained by the navigation solution deviates. However, due to the introduction of the zero-speed correction algorithm, the rate of error accumulation is greatly reduced, and it can maintain good performance for a certain period of time. The error distance and error ratio are shown in Table 1.

Table 1

Example 2

The pedestrian navigation method described above was applied to indoor experiments, as follows:

As shown in Figure 3, the four areas of ABCD are connected by corridors, so it walks clockwise along the wall at a constant speed, and finally returns to the starting position. The walking time is about 4 minutes and 02 seconds. Before starting to walk, first stand still at the starting position for 30 seconds to complete the initial alignment. The thin black line represents the displacement curve of the pedestrian navigation system using zero speed correction and magnetic heading assistance.

Since the traditional satellite positioning system is not used, the closed indoor environment does not affect the work of the pedestrian navigation system.

The error distance and error proportion are shown in Table 2. Due to the shortened walking time, the final error distance and error proportion data are reduced to a certain extent compared with long-distance walking.

Table 2

The above indoor and outdoor experiments all prove that the pedestrian navigation system designed in this project can meet the positioning and navigation needs of pedestrians within a certain period of time.

Claims (5)

1. A pedestrian navigation method based on inertial and magnetic heading and zero speed correction, is characterized in that, comprises the following steps: (1) Use inertial measurement elements to measure the linear motion information and angular motion information of pedestrians, and solve them through coordinate transformation and strapdown inertial navigation algorithm to obtain the pedestrian's attitude, speed and position information; Using the three-axis magnetic component measured by the magnetometer, the pedestrian's heading information is obtained through coordinate transformation and strapdown inertial navigation algorithm solution; (2) Through the zero-speed interval detection algorithm, the zero-speed state in the walking process is obtained, and the observation of the speed error is updated in this state, combined with the observation of the heading angle error, the Kalman filter equation is established to correct the error of the pedestrian navigation system , and finally complete the output of the corresponding navigation and positioning information; The state equation of the Kalman filter equation is as follows:

In the formula, X(t) is the system state quantity, F is the system state transition matrix, W(t) is the system noise; the specific form of the above formula is:

ω _ie is the angular velocity of the earth's rotation, L is the latitude value, R is the radius of curvature of the earth; g is the acceleration of gravity; δγ, δθ, δψ are roll angle error, pitch angle error and heading angle error in turn; δv _x , δv _y are eastward speed error and northward speed error respectively; δL is latitude error;

are the accelerometer random errors of the x-axis and y-axis respectively; ε _x , ε _y , ε _z are the gyroscope random errors of the x-axis, y-axis and z-axis respectively; Assuming that the first-order Markov process ε _r and the random constant ε _b constitute the random drift model of the gyroscope, then: ε=ε _b +ε _r ,

where ε is the measurement random error of the gyroscope, β is the anticorrelation time constant of the first-order Markov process, and w ₁ is the random noise; Assuming a first-order Markov process

constitutes the random error model of the accelerometer, namely

In the formula

is the measurement random error of the accelerometer, μ is the anti-correlation time constant, w ₂ is the random noise; The observed values select the observed values of eastward and northward speed errors and the observed values of heading angle errors, and the observation equation is: Z(t)=HX(t)+N(t) In the formula: Z(t) is the system observation quantity, H is the system observation matrix, N(t) is the system observation noise; the specific form of the above formula is:

In the formula

is the observed quantity of the horizontal velocity error, obtained from the horizontal velocity v _x and v _y output by the strapdown inertial navigation algorithm in the zero-speed state;

is the observed value of the heading angle error, obtained from the difference between the output of the magnetometer and the heading angle calculated by the strapdown inertial navigation algorithm; N _Vyaw is the observation noise of the horizontal velocity error, and N _ψ is the observation noise of the heading angle error ; After the system state equation and observation equation are established, the Kalman filter iteration can be carried out, so as to optimally estimate the system state quantity, obtain the navigation error, and then correct it. 2. the pedestrian navigation method based on inertial and magnetic heading and zero speed correction according to claim 1, it is characterized in that, in the step (2), the zero speed interval detection algorithm is: when the inertial measurement element outputs three at t _k moment The axis acceleration vector and |A(t _K )| indicate that the current foot movement state is in the zero-speed process, and then judge the acceleration vector and variance at the time t _k

Whether it indicates that the current foot movement state is in the zero-speed range, as follows: 2-1. The three-axis acceleration vector sum |A(t _K )| output by the inertial measurement element at time t _k satisfies:

Among them: k=1,2,...,n,n+1; a _x (t _k ), a _y (t _k ), a _z (t _k ) respectively refer to the X, Y output by the inertial measurement element at the time t _k , the component of the Z three-axis; The judgment formula of the three-axis acceleration vector sum |A(t _K )| in the process of zero speed is:

th _a is the judgment threshold of the three-axis acceleration vector and |A(t _k )|, g is the acceleration of gravity; 2-2. Acceleration vector and variance at time t _k

satisfy:

In the formula: n is the width of the sampling interval, which is selected according to the output characteristics of the inertial element,

is the average value of the acceleration vector sum in the sampling interval; Judgment formula of zero-speed process variance:

In the formula: th _b is the judgment threshold of the acceleration vector and variance at time t _k ; when the acceleration vector and variance at time t _k

When it is smaller than the set variance threshold th _b , the output of the judgment expression is 1, and it is determined that the human body is in a zero-speed state. 3. the pedestrian navigation method based on inertial and magnetic heading and zero-speed correction according to claim 2, characterized in that, the value of the judgment threshold _{th a} of triaxial acceleration vector sum |A(t _k )| is 0.46, The value of the variance threshold th _b is 0.28. 4. the pedestrian navigation method based on inertial and magnetic heading and zero speed correction according to claim 1, is characterized in that, the observation quantity of course angle error satisfies:

where ψ _Ins is the heading angle calculated by the strapdown inertial navigation algorithm, and Ψ _r is the heading angle output by the magnetometer after magnetic declination correction; ψ _r =ψ _m -ψ ₀ Ψ ₀ is the magnetic declination, Ψ _m is the magnetic heading angle calculated from the magnetic component measured by the three-axis magnetometer, specifically:

X _h ＝M _bx cosθ+M _by sinγsinθ+M _bz cosγsinθ Y _h ＝M _by cosγ-M _bz sinγ

are the three-axis magnetic components of the magnetometer in the pedestrian coordinate system, and θ and γ refer to the pitch angle and roll angle, respectively. 5. The pedestrian navigation method based on inertial and magnetic heading and zero-speed correction according to claim 1, wherein the inertial measurement element includes an accelerometer for measuring linear motion information and a gyroscope for measuring angular motion information.

Images