An inertial positioning method based on near-zero speed observation in a running state

Abstract

The present invention provides an inertial positioning method based on near-zero speed observation in a running state. An inertial sensor is fixed to the heel, and a rotation model of an "L"-shaped rigid body fixedly connected to the lower limb is introduced. Based on the decay characteristic of the pitch angular velocity, a peak detection method is used to determine the optimal near-zero point and near-zero interval of the speed, which can accurately and effectively detect the speed correction point of the pedestrian inertial navigation system in the running state. Then, the small speed at this time is estimated within the near-zero interval, and the small speed at the position where the sensor is located at this time is used as the observation quantity for Kalman filtering, which can effectively suppress the divergence of the system error in the running state and improve the inertial positioning accuracy.

Description

Technical Field

[0001] The present invention belongs to the field of pedestrian autonomous navigation and positioning, and particularly relates to an inertial positioning method based on near-zero velocity observation in a running state. Background Art

[0002] The pedestrian navigation and positioning technology based on inertial sensors is a key technology for realizing accurate positioning of personnel in the case of satellite denial. The positioning error of an inertial navigation system without correction diverges as time accumulates. The Zero-Velocity Update (ZUPT) algorithm utilizes the characteristic that there is a zero-velocity interval when a person walks, adopts a configuration scheme of inertial sensors fixed on the foot, introduces an observable with zero velocity, and performs Kalman filtering to estimate the system error, correct the system state, and improve the positioning accuracy. The fixed-threshold zero-velocity detection method and zero-velocity observation method adopted by the traditional ZUPT algorithm are difficult to adapt to the high-dynamic characteristics in the running state, which leads to the failure of the traditional ZUPT algorithm in the running state. At present, relevant studies have adopted methods such as adaptive thresholds, multi-sensor zero-velocity detection, and deep learning to solve the above problems. However, although these methods can achieve good results, they require continuous parameter adjustment, are not flexible, simple, and convenient enough, and have limited generalization ability, which is not conducive to popularization and application. Therefore, how to design a more simple, flexible, and stable inertial positioning algorithm in the running state is an urgent problem to be solved. Summary of the Invention

[0003] To solve the above problems, the present invention provides an inertial positioning method based on near-zero velocity observation in a running state, which has simplicity, convenience, and stability, can effectively suppress the error divergence of the inertial navigation system in the running state, and improve the inertial positioning accuracy in the running state.

[0004] An inertial positioning method based on near-zero velocity observation in a running state includes:

[0005] Step 1: During the landing process of the landing foot, take the contact point between the heel and the ground as the rotation center, and set the inertial sensor at a position close to the rotation center; where the distance between the inertial sensor and the rotation center is ΔR; when the human foot decelerates during landing, approximately regard the foot and the calf as an "L"-shaped rigid body connected fixedly.

[0006] Step 2: Detect the best near-zero point and the near-zero interval, specifically:

[0007] For the pitch angular velocity ω of the landing foot collected by the inertial sensor at each moment x(k) sequence, when the value of the pitch angular velocity at a certain moment is greater than the values at the previous and next moments, and the pitch angular velocity at this moment is negative, the pitch angular velocity at this moment is the peak point. Determine each peak point according to this rule and form the set Peak of the best near-zero points;

[0008] Take the pitch angular velocity ω x A number of data points before the best near-zero point in the (k) sequence as the near-zero interval;

[0009] Step 3: Estimate the small velocity of each point in the near-zero interval and perform the following calculations using the characteristics of the circular motion of the rigid body fixed to the person:

[0010]

[0011] where v(k) is the absolute velocity of the human foot at time k, v x (k), v y (k), v z (k) are the velocities in the x, y, and z-axis directions at time k in the navigation coordinate system; the navigation coordinate system is defined as the northeast celestial coordinate system;

[0012] At each sampling moment in the near-zero interval, use the inertial navigation system to calculate the pitch angle θ of the human foot relative to the ground, and then obtain the velocity component of the human foot in the z-axis direction in the navigation coordinate system:

[0013] v z (k) ≈ ε z (k) = v(k)sin(θ)

[0014] ε z (k) is defined as the small velocity in the z-axis direction at the current time k;

[0015] Then obtain the combined velocity of the x-axis and y-axis, that is, the horizontal component of the velocity is:

[0016]

[0017] ε xy (k) is defined as the small velocity in the combined direction of the x-axis and y-axis at the current time k;

[0018] Thus, according to the horizontal component of the velocity v xy (k), obtain the velocities of the human foot in the x-axis and y-axis directions in the navigation coordinate system at the current time k:

[0019]

[0020]

[0021] Among them, and respectively represent the mean values of the velocity in the x - direction and the mean value of the velocity in the y - direction calculated over a set time period before the current k - moment; ε x (k) and ε y (k) are respectively defined as the small velocities in the x - axis direction and the y - axis direction at the current moment k;

[0022] Step Four: In the near - zero interval, use the small velocity at the position of the inertial sensor at the k - moment as the observation quantity for Kalman filtering to correct the parameter values output by the inertial navigation system. Among them, the observation quantity is:

[0023]

[0024] p z (k) represents the self - height calculated by the inertial navigation system; v′ x (k), v′ y (k) and v′ z (k) respectively represent the velocities in the three coordinate axis directions at the k - moment calculated by the inertial navigation system.

[0025] Preferably, in the said Step One, ΔR is less than 10 centimeters.

[0026] Further, when detecting the optimal near - zero point, perform mean filtering on the pitch angular velocity ω x (k) of the landing foot collected by the inertial sensor at each moment, and use the filtered pitch angular velocity for detecting the optimal near - zero point.

[0027] Preferably, the window length of the said mean filtering is 3.

[0028] Preferably, in the said Step Two, the number of several data points is represents rounding down, and n represents the window length of the mean filtering.

[0029] Preferably, in the said Step Two, the peak points less than or equal to the set threshold ω t are removed from the set Peak.

[0030] Preferably, in the said Step Three, the set time is taken within 0.5 s.

[0031] The present invention has the following beneficial effects:

[0032] The present invention provides an inertial positioning method based on near-zero speed observation during running. The inertial sensor is fixed to the heel, and a rotation model of the "L"-shaped rigid body fixedly connected to the lower limb is introduced. Based on the decay characteristic of the pitch angular velocity, the peak detection method is used to determine the optimal near-zero point and near-zero interval of the speed, which can accurately and effectively detect the speed correction point of the pedestrian inertial navigation system during running. Then, the small speed at this time is estimated within the near-zero interval, and the small speed at the position where the sensor is located at this time is used as the observation quantity for Kalman filtering, which can effectively suppress the divergence of the system error during running and improve the inertial positioning accuracy. Description of the Drawings

[0033] Figure 1 It is a flowchart of an inertial positioning method based on near-zero speed observation during running provided by the present invention.

[0034] Figure 2 It is a schematic diagram of the configuration of the inertial sensor;

[0035] Figure 3 It is a schematic diagram of the heel landing and takeoff state;

[0036] Figure 4 It is a schematic diagram of the peak point detection method;

[0037] Figure 5 It is a trajectory comparison diagram of the effects of the method of the present invention and the traditional method. Detailed Embodiments

[0038] In order to enable those skilled in the art of the present technology to better understand the solution of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present application.

[0039] See Figure 1 , which is a flowchart of an inertial positioning method based on near-zero speed observation during running provided by this embodiment. An inertial positioning method based on near-zero speed observation during running includes the following steps:

[0040] S1: As Figure 2 shown in the schematic diagram of the configuration of the inertial sensor, during the process of the landing foot touching the ground, the contact point between the heel and the ground, i.e., the rotation center, is the ideal zero-speed part. However, due to the size limitation and installation limitation of the inertial sensor, the inertial sensor cannot be configured at the rotation center. Therefore, there is a deviation ΔR (ΔR is less than 10 cm) between the currently configured position and the ideal zero-speed part;

[0041] S2: As Figure 2The figure shows a schematic diagram of the liftoff and landing states when the heel of the human body touches the ground. When decelerating during landing, the human foot and calf can be approximately regarded as an "L"-shaped rigid body connected firmly. From the decelerating state when the single foot touches the ground until the sole of the foot touches the ground, it can be regarded as a process in which the "L"-shaped rigid body rotates around point O1. During this process, the entire rigid body is in a decelerating state (in actual situations, after the heel touches the ground, it first rotates and accelerates and then decelerates rapidly). Point O1 on the heel is the rotation center point of the rigid body rotation, and O1 is the ideal zero-speed position. Since during the running state, the rotational angular velocity of the "L"-shaped rigid body will not decay to zero, therefore, actually in the rotation deceleration interval, the position where the sensor is located is not in a truly zero-speed state. However, as the foot surface gradually approaches the ground, the position where the sensor is located will gradually approach the zero-speed state. When the landing foot surface completely touches the ground and is on the same straight line as the human torso (that is, the projection of the head on the ground coincides with the foot), the position where the inertial sensor is located will reach the state closest to zero speed.

[0042] S3: Define that if there exists a moment t0, satisfying that the landing foot surface completely touches the ground and is on the same straight line as the human torso, then the neighborhood centered at t0 with a radius of δ is called the near-zero interval. In the near-zero interval, the speed of the landing foot is very small. In the interval (t0 - δ, t0), although the speed of the landing foot is very small, its rotation center is still O1, and the speed shows a decreasing trend. In the interval (t0, t0 + δ), the speed of the landing foot is still very small, but its rotation center becomes O2, and the speed shows an increasing trend. All moments in this interval are called near-zero points. The speed of the foot is closest to zero at moment t0. Therefore, moment t0 is called the best near-zero point in the near-zero interval.

[0043] S4: Traditional correction algorithms based on zero-speed observation rely on the accuracy of zero-speed detection. Since the human body has a relatively long zero-speed interval and a relatively low movement speed during normal movement, some misdetections and missed detections at certain points will not have a very significant impact on the results. However, when the human body is in a running state, there is no truly zero-speed interval, only a near-zero interval with a small speed. Since the human body movement is in a relatively high-speed and high-step-frequency state, the detection of the near-zero interval with a small speed and the estimation of the small speed will greatly affect the final positioning result. Therefore, for the detection of the near-zero interval in the present invention, first, the best near-zero point is found by using the angular velocity decay characteristic, and then a few points are traced back from the best near-zero point as the starting point as the near-zero interval. Then, according to the established rotation model, the small speeds of each point in the near-zero interval are calculated by using the angular velocities of each point in the near-zero interval, and this small speed is used as the observation quantity to correct each state variable of the inertial navigation system by using the Kalman filter.

[0044] The problem of detecting the best near-zero point is essentially a peak detection problem. In order to effectively avoid interference, the present invention first performs a mean filtering on the original signal collected by the inertial sensor:

[0045]

[0046] After that, a sliding window with a length of 3 is used for sampling to detect peaks. As Figure 4 shown, when the value of the pitch angular velocity at a certain moment is greater than the values at the previous and next moments, and the pitch angular velocity at this moment is negative, the pitch angular velocity at this moment is a peak point. According to this rule, each peak point is determined to form the set Peak of the best near-zero points, that is:

[0047]

[0048] where ω x (k) is the pitch angular velocity measured by the inertial sensor at time k, is the pitch angular velocity of the inertial sensor after mean filtering, and n is the window length of mean filtering; to prevent the occurrence of abnormal near-zero points, it is required that the peak point is greater than a negative set threshold ω t .

[0049] Using this method may detect multiple peak points in the peak part of the signal, but as long as it is between the deceleration interval and the acceleration interval, no further adjustment is required. Starting from the detected peak points, points can be continuously taken forward and backward to determine a near-zero interval. Since the peak point after mean filtering lags behind the true peak point, the true best near-zero point should be in the front. To ensure that the obtained near-zero interval contains the best near-zero point, and at the same time to ensure that the rotation center is O1, it is necessary to trace back several points forward and retain all of them, and then determine a near-zero interval. Generally, the number of points to trace back is denotes rounding down.

[0050] S5: Using the near-zero interval speed correction can avoid the error caused by the detection error of the best near-zero point. As can be seen from the above, when the position of the inertial sensor is near the near-zero interval, although its speed is not really zero, it is also very small. However, if it is forcibly set to zero, it will cause the mileage estimation to be too small. Therefore, it is necessary to estimate the small speeds of each point in the near-zero interval and perform the following calculations using the characteristics of the circular motion of the rigid body fixed connection:

[0051]

[0052] where v(k) is the absolute speed of the human foot at time k, v x (k), v y (k), v z (k) are the speeds in the three directions in the navigation coordinate system; the navigation coordinate system is defined as the northeast celestial coordinate system;

[0053] In the near-zero interval, the pitch angle θ of the human foot relative to the ground at time k is obtained by solving the inertial navigation system INS, and the velocity component in the z-axis direction in the navigation coordinate system is:

[0054] v z (k)≈ε z (k)=v(k)sin(θ)

[0055] ε z (k) is defined as the small velocity in the z-axis direction at the current time k;

[0056] Then, the combined velocity of the x-axis and y-axis is obtained, that is, the horizontal component is:

[0057]

[0058] ε xy (k) is defined as the small velocity in the combined direction of the x-axis and y-axis at the current time k;

[0059] Thus, according to the combined velocity v xy (k), the velocities of the human foot in the x-axis and y-axis directions in the navigation coordinate system at the current time k can be obtained:

[0060]

[0061]

[0062] Among them, and respectively represent the mean values of the x-direction velocity and y-direction velocity calculated in a period of time (within 0.5 s) before the current time k; ε x (k) and ε y (k) are respectively defined as the small velocities in the x-axis and y-axis directions at the current time k;

[0063] S6: Using the small velocity at the position of the inertial sensor at time k as the observation quantity for Kalman filtering to correct the parameter values output by the inertial navigation system. Among them, using the navigation system error as the state variable X = δx = [δp T δv T δψ T T , using the velocity error and altitude error as the observation quantities Z = [δp z δv x δv y δv z T , establish the system state equation and measurement equation:

[0064] X k =Φ k|k-1 X k-1 +Γ​​k-1 w k-1

[0065] Z k Z = HX k|k + v k

[0066] where Φ k|k-1 is the state transition matrix from time k - 1 to k, Γ k-1 is the system noise driving matrix, H is the measurement matrix, w k-1 is the system excitation noise matrix, v k is the measurement noise matrix.

[0067] In the observation equation, there is the following relationship:

[0068]

[0069] where k is the time of each scatter point in the near - zero interval, p z (k) represents the self - height calculated by the inertial navigation system; v′ x (k), v′ y (k) and v′ z (k) respectively represent the velocities in the three coordinate axis directions at time k calculated by the inertial navigation system; ε x (k), ε y (k), ε z (k) are the small velocities in the three coordinate axis directions estimated at this time, and ΔR is the distance from the installation position of the inertial sensor to the heel.

[0070] Here, it is considered that the error can be approximated to zero after each correction. Therefore, there is no need to use the time update equation to propagate the error state. Instead, the Kalman filter equation is established to estimate the system error and then correct the system state:

[0071] X k = K k Z k

[0072]

[0073]

[0074] P k = [I - K k H k P k,k-1

[0075] Figure 5 As shown in the personnel running experiment on the playground, the invented method can effectively improve the inertial positioning accuracy in the running state.

[0076] This embodiment not only effectively improves the inertial positioning accuracy during running, but also enhances the simplicity, convenience and stability of the method, which is conducive to the popularization of the method.

[0077] Of course, the present invention may also have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art can certainly make various corresponding changes and deformations according to the present invention. However, these corresponding changes and deformations should all fall within the protection scope of the appended claims of the present invention.

Claims

1. An inertial positioning method based on near-zero speed observation in a running state, characterized in that Including: Step 1: During the process of the landing foot touching the ground, take the contact point between the heel and the ground as the rotation center, and set the inertial sensor at a position close to the rotation center; where the distance between the inertial sensor and the rotation center is ΔR; when the human foot decelerates during landing, approximately regard the foot and the calf as an "L"-shaped rigid body connected fixedly. Step 2: Detect the optimal near-zero point and the near-zero interval, specifically: For the pitch angular velocity ω of the landing foot collected by the inertial sensor at each moment x (k) sequence, when the value of the pitch angular velocity at a certain moment is greater than the values at its previous and next moments, and the pitch angular velocity at this moment is negative, the pitch angular velocity at this moment is a peak point. Determine each peak point according to this rule and form the set Peak of the best near-zero points; Obtain the pitch angular velocity ω x Use several data points before the best near-zero point in the (k) sequence as the near-zero interval; Step 3: Estimate the small velocities of each point in the near-zero interval, and perform the following calculations using the characteristics of the circular motion of the fixedly connected rigid body: where v(k) is the absolute velocity of the human foot at time k, and v x (k), v y (k), v z (k) are the velocities in the x, y, and z-axis directions at time k in the navigation coordinate system; the navigation coordinate system is defined as the northeast-up coordinate system; At each sampling moment in the near-zero interval, use the inertial navigation system to calculate the pitch angle θ of the human foot relative to the ground, and then obtain the velocity component of the human foot in the z-axis direction in the navigation coordinate system: v z (k) ≈ ε z (k) = v(k)sin(θ) ε z (k) is defined as the small velocity in the z-axis direction at the current moment k; Then obtain the combined velocity of the x-axis and the y-axis, that is, the horizontal component of the velocity is: ε xy (k) is defined as the small velocity in the combined direction of the x-axis and the y-axis at the current k-th moment; Thus, based on the horizontal component of velocity v xy (k), the velocities in the x-axis and y-axis directions of the human foot in the navigation coordinate system at the current moment k are obtained: Among them, and respectively represent the mean values of the velocity in the x - direction and the mean value of the velocity in the y - direction calculated for a set time period before the current k - th moment; ε x (k) and ε y (k) are respectively defined as the small velocities in the x - axis direction and the y - axis direction at the current moment k; Step 4: In the near-zero interval, use the small velocity at the position of the inertial sensor at the kth moment as the observation quantity for Kalman filtering to correct the parameter values output by the inertial navigation system, where the observation quantity is: p z (k) represents the own height calculated by the inertial navigation system; v′ x (k), v′ y (k) and v′ z (k) respectively represent the velocities in the three coordinate axis directions at the k-th moment calculated by the inertial navigation system.

2. The inertial positioning method based on near-zero speed observation in a running state according to claim 1, wherein In the said Step 1, ΔR is less than 10 centimeters.

3. The inertial positioning method based on near-zero speed observation in a running state according to claim 1, wherein When detecting the best near-zero point, the mean filtering is performed on the pitch angular velocity ω x (k) of the landing foot collected by the inertial sensor at each moment, and the best near-zero point is detected by using the filtered pitch angular velocity.

4. The inertial positioning method based on near-zero speed observation in a running state according to claim 3, wherein, The window length of the said mean filtering is 3.

5. The inertial positioning method based on near-zero speed observation in a running state according to claim 3, characterized in that In the second step, the number of several data points is represents rounding down, and n represents the window length of mean filtering.

6. The inertial positioning method based on near-zero speed observation in a running state according to claim 1, wherein In the second step, those with peak points less than or equal to the set threshold ω t are removed from the set Peak.

7. The inertial positioning method based on near-zero speed observation in a running state according to claim 1, wherein In the said Step 3, the set time is within 0.5 s.

Images