Gait feature based micro-inertial navigation kalman filter parameter adaptive setting method

Abstract

This invention discloses a method for adaptively setting parameters for a micro-inertial navigation Kalman filter based on gait characteristics. The method involves attaching a micro-inertial sensor to a pedestrian's foot to collect raw inertial data during gaits such as straight-line walking, arc walking, and turning. The mean heading angle calculated within each step's zero-speed interval is used to calculate the heading angle difference between adjacent steps. This heading angle difference is used to determine the pedestrian's gait. When the pedestrian walks in an arc or turns, the heading angle difference is used to adaptively adjust the value of the system noise matrix Q, and the measurement noise matrix R using new information. This method adaptively adjusts the system noise and measurement noise of the Kalman filter, significantly improving error estimation accuracy and enabling high-precision positioning and navigation for pedestrians.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of foot-mounted pedestrian navigation based on micro-inertial sensors, and relates to a micro-inertial navigation Kalman filter parameter adaptive setting method based on gait characteristics. BACKGROUND

[0002] In the conventional foot-mounted pedestrian navigation algorithm, the values of the filter Q matrix and R matrix are constant when the Kalman filter is used to correct the navigation error, but the attitude error estimation accuracy and the speed observation accuracy are different under different gaits such as straight walking, arc walking and turning, and the accuracy of error correction using the fixed parameter filter will decrease. SUMMARY

[0003] In view of the problem that the accuracy of error correction using the fixed parameter filter will decrease, the application aims to provide a micro-inertial navigation Kalman filter parameter adaptive setting method based on gait characteristics, which can improve the accuracy of filter correction and thus improve the positioning accuracy.

[0004] To achieve the object of the application, the application provides a micro-inertial navigation Kalman filter parameter adaptive setting method based on gait characteristics, which adopts the following technical solutions:

[0005] The micro-inertial sensor is bound to the foot of the pedestrian, and original inertial data of the pedestrian under different gaits such as straight walking, arc walking and turning are collected; the mean value of the heading angle in each step zero speed interval is calculated to obtain the heading angle difference value of adjacent steps; the gait of the pedestrian is determined by using the heading angle difference value; when the pedestrian is walking in an arc or turning, the value of the system noise Q matrix is adaptively adjusted by using the heading angle difference value, and the value of the measurement noise R matrix is adaptively adjusted by using the innovation.

[0006] Further, the gait of the pedestrian is determined by using the heading angle difference value of adjacent steps, and the method is as follows: a gait determination threshold is set, when the heading angle difference value of adjacent steps is less than the set threshold, it indicates that the pedestrian is in a straight walking state, and when the heading angle difference value of adjacent steps is greater than the set threshold, it indicates that the pedestrian is in an arc walking or turning state.

[0007] Further, when the pedestrian is performing an arc walking or turning maneuver, the value of the attitude error corresponding to the Q matrix is adaptively adjusted as follows:

[0008]

[0009] wherein i=1, 2, 3, q i is the adjusted Q matrix diagonal element value, is the heading angle difference value between two adjacent steps, represents the noise correction amount introduced according to the motion intensity, and Δ base represents the modified base threshold, which is a constant value.

[0010] Further, when the pedestrian is in the arc walk or turning maneuver motion, the new information is used to adaptively adjust the R matrix:

[0011] R i ′=R i +Rk*Rk

[0012] Wherein, i=1, 2, 3, R i ′ is the diagonal element value of the adjusted R matrix, and the R matrix is limited to set:

[0013]

[0014] Wherein, Th R Is the R matrix amplitude threshold.

[0015] The beneficial effects of the present application compared with the prior art are as follows:

[0016] The present application fixes the micro-inertial sensor on the human foot, and updates the pedestrian navigation information in real time through the strapdown inertial navigation solution, and uses the Kalman filter based on the speed error observation to suppress the navigation error divergence. In order to exploit the pedestrian gait characteristics such as straight walk, arc walk and turning, the difference between the adjacent step heading angles is used to judge the motion state, the Q matrix and the R matrix of the Kalman filter are adaptively adjusted based on the gait discrimination result, the precision of the filter correction is improved, and the positioning precision is improved. The present application is especially suitable for solving the pedestrian high-precision positioning and navigation problem in urban environment such as building and tunnel. BRIEF DESCRIPTION OF DRAWINGS

[0017] The included drawings are used to provide further understanding of the embodiments of the present application, and constitute a part of the specification, used to illustrate the embodiments of the present application, and together with the text description to explain the principle of the present application. Obviously, the drawings in the following description only show some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0018] Figure 1 The basic principle block diagram of the micro-inertial navigation Kalman filter parameter adaptive setting method based on gait characteristics provided by the specific embodiment of the present application is shown. DETAILED DESCRIPTION

[0019] It should be noted that the embodiments and features in the present application can be combined with each other in the case of no conflict. The technical solutions in the embodiments of the present application will be described clearly and completely in combination with the drawings of the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. The description of the at least one example embodiment is actually only illustrative, but not as any limitation on the present application and its application or use. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0020] The method for adaptively setting parameters of a Kalman filter for micro-inertial navigation based on gait characteristics provided by the embodiments of the present application includes the following contents:

[0021] The micro-inertial sensor is bound to the foot of a pedestrian, and original inertial data of the pedestrian under straight walking, arc walking, turning and other gaits are collected; the mean value of the heading angle calculated in each step zero speed interval is used to calculate the heading angle difference value of adjacent steps; the heading angle difference value is used to judge the gait of the pedestrian; when the pedestrian walks in an arc or turns, the value of the system noise Q matrix is adaptively adjusted by using the heading angle difference value, and the value of the measurement noise R matrix is adaptively adjusted by using the innovation.

[0022] As shown in Figure 1 , the specific method of the present application includes the following steps:

[0023] Step 1. Inertial data collection

[0024] The micro-inertial sensor is bound to the foot of a pedestrian, and original inertial data under various walking states are collected, including straight walking, arc walking, turning and other actions.

[0025] Step 2. Zero speed interval detection

[0026] There is a period of periodic foot-ground contact in each single step of the pedestrian during the movement, and the movement speed of the foot of the pedestrian in this period is zero, which is called a zero speed interval. The threshold method, sliding variance method, generalized likelihood ratio detection and other methods are used to find the zero speed interval of each single step.

[0027] Step 3. Adjacent step heading angle difference value calculation

[0028] Suppose the start and end time of the zero speed interval of the kth single step are t k,start and t k,end , respectively, and the zero speed interval contains n k inertial data sampling time points, wherein the heading angle at time i is The start and end time of the zero speed interval of the k+1th single step are t k+1,start and t k+1,end, the zero speed interval contains n k+1 Inertial data sampling moments, where the heading angle at moment i is Then the heading angles of the kth step and the k+1th step can be expressed as:

[0029]

[0030]

[0031] Then the heading angle difference between the two adjacent steps k and k+1 is It can be expressed as:

[0032]

[0033] Step 4. Pedestrian gait identification

[0034] The pedestrian gait is judged by the difference of the heading angles of adjacent steps. heading is the gait discrimination threshold, when satisfy:

[0035]

[0036] Indicates that the pedestrian is walking in a straight line.

[0037] when satisfy:

[0038]

[0039] Indicates that the pedestrian is walking in an arc or turning.

[0040] Step 5. Kalman filter Q matrix adaptive setting

[0041] Assume that the system state vector of the Kalman filter is:

[0042]

[0043] Among them, δV n , δV u , δV e They represent the north, sky and east velocity errors of the micro-inertial navigation system respectively; δL, δh, δλ represent the latitude error, height error and longitude error respectively; δγ, δθ represent the roll angle error, heading angle error, and pitch angle error respectively.

[0044] The system state equation is:

[0045]

[0046] Wherein, F(t) is a state transition matrix, W(t) is a system noise vector, satisfying:

[0047] E[W k ]=0,Cov[W k ,W j ]=E[W k W j T ]=Qδ kj

[0048] Wherein, W k , W j are noise vectors at k, j time respectively, δ kj represents a unit matrix, Q matrix is a system noise variance matrix of Kalman filter, wherein, the diagonal elements corresponding to roll angle error, heading angle error, pitch angle error are constant respectively q1, q2, q3.

[0049] When the pedestrian walks in a straight line, q1, q2, q3 accurately reflect the device noise characteristics under the condition of regular motion, and the value is constant.

[0050] When the pedestrian is in arc walk or turning, etc. Dynamic characteristics of the pedestrian are enhanced, the motion amplitude is increased, the inertial navigation is enhanced, the attitude error estimation accuracy is improved, and the corresponding noise value should be adaptively reduced. At this time, the value of the corresponding Q matrix of the attitude error is adaptively adjusted as follows:

[0051]

[0052] Wherein, q i '(i=1, 2, 3) is the diagonal element value of the adjusted Q matrix, Indicates the noise correction introduced according to the motion intensity, Δ base Indicates the modified base threshold, which is a constant.

[0053] Step 6. Adaptive setting of Kalman filter R matrix

[0054] The measurement Z of Kalman filter is the speed error in the zero speed interval, that is:

[0055] Z=[V n -0 V u -0 V e -0] T =[V n V u V e ] T

[0056] Let R matrix be the measurement noise variance matrix corresponding to three-dimensional measurement, and its diagonal elements are constant values R1, R2 and R3 respectively.

[0057] By one-step prediction The estimation error (i.e. innovation) caused by the real state X to the measurement is:

[0058]

[0059] Where H is the system measurement matrix.

[0060] When the pedestrian walks in a straight line, the speed error observation accuracy is high due to small motion amplitude and relatively stable gait, and the R matrix is a constant value set.

[0061] When the pedestrian performs a maneuver such as arc walking or turning, the motion becomes more intense, resulting in a decrease in speed error observation accuracy, and the corresponding noise value should be adaptively increased. At this time, the innovation is used to adaptively adjust the R matrix:

[0062] R i ′=R i +Rk*Rk,(i=1,2,3)

[0063] Where R i ′(i=1,2,3) is the value of the diagonal element of the adjusted R matrix. At the same time, the R matrix is limited to setting:

[0064]

[0065] Where Th R is the R matrix amplitude limiting threshold.

[0066] Finally, it should be pointed out that: the above examples are only used to illustrate the technical solutions of the present application, and not to limit it; although the present application has been described in detail with reference to the foregoing examples, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement to part of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A method for adaptively setting parameters of micro-inertial navigation Kalman filter based on gait characteristics, characterized in that: The micro-inertial sensor is attached to the pedestrian's foot to collect raw inertial data during the pedestrian's gait. The heading angle difference between adjacent steps is calculated using the average of the heading angles within the zero-speed interval of each step. The heading angle difference is used to determine the pedestrian's gait. When a pedestrian walks in an arc or turns, the value of the system noise Q matrix is ​​adaptively adjusted using the heading angle difference, and the value of the measurement noise R matrix is ​​adaptively adjusted using the new information. When a pedestrian performs an arc or turns maneuver, the value of the Q matrix corresponding to the posture error is adaptively adjusted to: Where i = 1, 2, 3, q i ′ is the diagonal element value of the Q matrix after adjustment, is the heading angle difference between two adjacent steps, Indicates the noise correction amount introduced according to the intensity of the movement, Δ base Represents the corrected base threshold, which is a constant value.

2. The method for adaptively setting parameters of a micro-inertial navigation Kalman filter based on gait characteristics according to claim 1, wherein: The gait of pedestrians is judged by using the difference in heading angles of adjacent steps. The method is as follows: a gait discrimination threshold is set. When the difference in heading angles of adjacent steps is less than the set threshold, it indicates that the pedestrian is walking in a straight line. When the difference in heading angles of adjacent steps is greater than the set threshold, it indicates that the pedestrian is walking in an arc or turning.

3. The method for adaptively setting parameters of micro-inertial navigation Kalman filter based on gait characteristics according to claim 1, characterized in that: When pedestrians perform arc walking or turning maneuvers, the R matrix is ​​adaptively adjusted using the new information: R i ′=R i +Rk*Rk Where i = 1, 2, 3, R i ′ is the diagonal element value of the adjusted R matrix, and the R matrix is ​​limited at the same time: Among them, Th R is the R matrix clipping discrimination threshold.

Images