A pedestrian navigation method based on interfoot distance constraint

Abstract

The application provides a pedestrian navigation method based on inter-foot distance constraint, and threshold values of inter-foot distance constraint are set for normal walking and running states respectively; two micro inertial sensors are bound to the left foot and the right foot of a pedestrian respectively, output data of any micro inertial sensor is collected, and the motion state is determined as normal walking or running; the inter-foot distance at the current time is calculated, the position coordinates of the two feet are updated according to the relationship between the inter-foot distance and the threshold value in different states, and the navigation error is estimated and corrected by using Kalman filtering. The application can improve the position updating precision by adaptively adjusting the inter-foot distance threshold value, thereby improving the error correction precision.

Description

Technical Field

[0001] This invention belongs to the field of pedestrian navigation technology, specifically relating to a pedestrian navigation method based on interfoot distance constraints. Background Technology

[0002] The bipedal micro-inertial navigation system obtains the navigation results for a single foot through strapdown inertial navigation calculations and uses a zero-velocity correction method to correct navigation errors. Simultaneously, it utilizes inter-foot distance constraints to suppress heading error divergence. Traditional pedestrian navigation algorithms based on bipedal micro-inertial sensors maintain a constant distance threshold when correcting navigation errors using inter-foot distance constraints. However, when a pedestrian is running, both feet may be airborne, resulting in a true inter-foot distance greater than the set threshold, leading to miscorrections and reduced positioning accuracy. Summary of the Invention

[0003] To address the technical problem of low positioning accuracy caused by fixed foot distance constraints in existing technologies, this invention provides a pedestrian navigation method based on foot distance constraints. Different foot distance constraint thresholds are set for normal walking and running phases, thereby accurately updating the position coordinates. A Kalman filter is used to suppress navigation error divergence, improving the pedestrian navigation accuracy based on bipedal micro-inertial sensors under variable step frequency motion conditions.

[0004] The technical solution adopted by the present invention to solve the above-mentioned technical problems is as follows:

[0005] A pedestrian navigation method based on inter-foot distance constraints includes the following steps:

[0006] Set a first threshold for the interfoot distance constraint during normal walking;

[0007] Set the distance between feet to a first threshold when one foot touches the ground during running, and the distance between feet to a second threshold when both feet are off the ground.

[0008] Two micro-inertial sensors are attached to the left and right feet of a pedestrian, respectively. Data from the output of either micro-inertial sensor is collected to determine whether the movement is normal walking or running.

[0009] In the running state, it is judged as either one foot touching the ground or both feet in the air.

[0010] Calculate the distance between the feet at the current moment;

[0011] In normal walking mode, if the distance between the feet is greater than a first threshold, the coordinates of both feet are updated based on the first threshold.

[0012] When running with one foot on the ground, if the distance between the feet is greater than a first threshold, then update the coordinates of both feet based on the first threshold.

[0013] When both feet are off the ground during running, if the distance between the feet is greater than the second threshold, then the coordinates of the feet are updated based on the second threshold.

[0014] Kalman filtering is used to estimate and correct navigation errors.

[0015] Furthermore, the first threshold is calculated based on the pedestrian's leg length, and the formula for calculating the first threshold is:

[0016] γ1=a·l

[0017] Where l is the pedestrian's leg length and a is the proportionality coefficient.

[0018] Furthermore, the method for determining the proportionality coefficient 'a' is as follows:

[0019]

[0020] Where h is the pedestrian's height.

[0021] Furthermore, the second threshold is calculated based on the pedestrian's leg length and take-off height, and the formula for calculating the second threshold is:

[0022]

[0023] Where l is the pedestrian's leg length, and Δh is the relative height of the foot to the ground obtained through inertial navigation calculation.

[0024] Furthermore, the method for determining the motion state includes the following steps:

[0025] Select a micro inertial sensor and calculate its triaxial resultant acceleration;

[0026] Set a first sliding window and calculate the variance of the triaxial resultant acceleration of the micro inertial sensor in the first sliding window;

[0027] If the variance is greater than the set variance threshold, the pedestrian is in a running state; otherwise, they are in a normal walking state.

[0028] Furthermore, the formula for calculating the variance of the triaxial resultant acceleration is as follows:

[0029]

[0030] Among them, a i The i-th triaxial resultant acceleration within the first sliding window. It is the mean of the three-axis resultant acceleration within the first sliding window, and n is the length of the first sliding window.

[0031] Furthermore, the method for determining whether running involves one foot touching the ground or both feet being off the ground is as follows:

[0032] Collect data from the micro-inertial sensors on both feet to determine if one foot is at zero speed. If so, the current state is one foot on the ground; otherwise, both feet are off the ground.

[0033] Furthermore, the condition for satisfying the zero-velocity state is:

[0034]

[0035] Among them, a y (k) represents the measured Y-axis acceleration at time k, ω k The resultant angular velocity of the three axes of the micro inertial sensor. Y-axis acceleration threshold This is the angular velocity threshold.

[0036] Furthermore, the method for calculating the distance between the feet at the current moment is as follows:

[0037]

[0038] in, These represent the latitude, altitude, and longitude obtained from the left foot inertial navigation system at time k. These represent the latitude, altitude, and longitude calculated by the right foot inertial navigation system at time k, respectively. M R N Let L be the Earth's principal curvature radius and L be the local latitude.

[0039] Furthermore, the method for updating the coordinates of the two feet is as follows:

[0040]

[0041]

[0042] in, These are the updated position coordinates of the left and right feet, respectively. These are the left and right foot position coordinates obtained from the inertial navigation system calculation, γ1 and γ2 are the first and second thresholds, respectively, and d k This refers to the distance between the feet.

[0043] The beneficial effects of this invention compared to the prior art are as follows:

[0044] To improve the correction effect of foot distance constraints under different paces, this invention sets different foot distance thresholds for walking and running states, updates position coordinates, obtains position error observations, and suppresses error divergence using a Kalman filter. This method improves position update accuracy by adaptively adjusting the distance threshold, thereby enhancing error correction accuracy. This invention is particularly suitable for solving the bipedal constrained pedestrian navigation problem under varying stride frequencies. Attached Figure Description

[0045] The accompanying drawings, which form part of this specification, are provided to further illustrate embodiments of the invention and, together with the textual description, explain the principles of the invention. It is obvious that the drawings described below are merely some embodiments of the invention, and those skilled in the art can obtain other drawings based on these drawings without any creative effort.

[0046] Figure 1 A flowchart of a pedestrian navigation method based on interfoot distance constraints provided for a specific embodiment of the present invention. Detailed Implementation

[0047] Specific embodiments of the present invention will now be described in detail. In the following description, specific details are set forth for purposes of explanation and not limitation, in order to aid in a thorough understanding of the invention. However, it will be apparent to those skilled in the art that the invention may be practiced in other embodiments departing from these specific details.

[0048] It should be noted that, in order to avoid obscuring the invention with unnecessary details, only the device structure and / or processing steps closely related to the solution of the invention are shown in the accompanying drawings, while other details that are not closely related to the invention are omitted.

[0049] To meet the application requirements under different pace speeds, different interfoot distance thresholds are set for walking and running states. Position coordinates are updated based on these thresholds, and position error observations are obtained. A Kalman filter is then used to correct navigation errors, further improving positioning accuracy. This invention employs the above method to improve pedestrian navigation accuracy under variable stride frequency movement states.

[0050] As one aspect of the present invention, a pedestrian navigation method based on interfoot distance constraints is provided. For example... Figure 1 As shown, two micro-inertial sensors are attached to the left and right feet of a pedestrian, respectively, to collect raw inertial data. The sliding window variance method is used to determine whether the pedestrian is in a normal walking or running state. During the normal walking phase, the inter-foot distance constraint threshold is set to a fixed threshold related to the pedestrian's leg length. During the running phase when both feet are in the air, the inter-foot distance constraint threshold is set to a variable threshold obtained through trigonometric function relationships. The position coordinates are updated using the inter-foot distance constraint, and the position error observation is obtained. The navigation error is then corrected using a Kalman filter.

[0051] 1. Gait discrimination

[0052] Two micro-inertial sensors were attached to the left and right feet of a pedestrian, respectively, to collect raw inertial data at different speeds, such as normal walking and running, and to determine the motion state. Since variance reflects the degree of dispersion of data, in normal walking, foot movement is stable and the acceleration variance is small, while in running, foot movement is vigorous and the acceleration variance is large. Therefore, the sliding window variance method is used to determine the motion state.

[0053] Suppose that the triaxial acceleration vector and magnitude (i.e., the resultant triaxial acceleration) measured at any time k for a certain foot are:

[0054]

[0055] Among them, a x (k), a y (k), a z (k) represents the measured values ​​of X, Y, and Z axis accelerations at time k.

[0056] The variance of the triaxial resultant acceleration of this foot is calculated using a sliding window:

[0057]

[0058] Among them, a i Let i be the resultant acceleration of the i-th triaxial axis within the sliding window. It is the average of the three-axis acceleration vectors and magnitudes within the sliding window, where n is the length of the sliding window, which is set according to the sampling frequency of the micro inertial sensor.

[0059] Then when When, it indicates that the pedestrian is in a normal walking state; when When λ is the time, it indicates that the pedestrian is in a running state. Here, λ is the variance threshold.

[0060] 2. Set the interfoot distance constraint threshold during the normal walking phase.

[0061] During normal walking, when one foot is stationary and touching the ground, the other foot is swinging, and the distance between the two feet does not exceed a fixed threshold, which is defined as the foot-to-foot distance constraint threshold. Traditionally, this threshold is a fixed value, but in reality, it is related to human physiological characteristics and is positively correlated with leg length. Therefore, it is set as follows:

[0062] γ1=a·l

[0063] Where γ1 is the interfoot distance constraint threshold in walking mode, l is the pedestrian's leg length, and a is a proportionality coefficient, a fixed value within the interval [0.5, 0.8] and related to the pedestrian's height. Since the height of an average adult is generally between 1.4m and 2.2m, the value of a is determined by the following formula:

[0064]

[0065] Where h is the pedestrian's height.

[0066] Suppose that the position vectors calculated by the left and right inertial navigation systems at any time k are respectively:

[0067]

[0068]

[0069] in, These represent the latitude, altitude, and longitude obtained from the left foot inertial navigation system at time k. These represent the latitude, altitude, and longitude obtained from the right foot inertial navigation system at time k.

[0070] Therefore, the distance between the feet at any time k can be expressed as:

[0071]

[0072] Among them, R M R N Let L be the Earth's principal curvature radius, and L be the local true latitude. This can be derived from... or Replacement.

[0073] Therefore, the interfoot distance constraint during normal walking is expressed as:

[0074] d k ≤γ1

[0075] 3. Set the interfoot distance constraint threshold during the running phase.

[0076] During the running phase, pedestrians may experience a state where both feet leave the ground and are airborne due to their fast walking speed. At this time, the distance between the feet increases and may exceed the set threshold γ1. Resetting the position coordinates will increase the error, so it is necessary to reset the distance between the feet constraint threshold.

[0077] First, determine the foot-free state. Foot-free means neither foot is touching the ground, meaning they are not at zero velocity. If at time k, the acceleration and angular velocity of any foot satisfy the following condition:

[0078]

[0079] This indicates that the foot is at zero speed. Where ω k This refers to the vector sum and magnitude of the three-axis angular velocities (i.e., the resultant angular velocity of the three axes). Y-axis acceleration threshold This is the angular velocity threshold. The threshold can be determined through statistical experiments; in this embodiment, Take 9.2 m / s respectively 2 and 10.4 m / s 2 , Take 0.15 rad / s.

[0080] If neither foot meets the above conditions, it indicates that the pedestrian is in a state of both feet in the air, and the distance constraint threshold needs to be reset. If we consider the human leg as a lever of fixed length, and then calculate the height of the foot from the ground when both feet are in the air using inertial navigation, we can use trigonometric functions to obtain the new foot distance constraint threshold as follows:

[0081]

[0082] Where γ2 is the variable foot-to-foot distance constraint threshold during running, and Δh is the relative height of the foot to the ground obtained by inertial navigation calculation.

[0083] Therefore, the interfoot distance constraint during the running phase is expressed as:

[0084]

[0085] 4. Error Suppression Based on Interfoot Distance Constraints

[0086] Assume the interfoot distance d calculated at time k k Greater than the set threshold γ i (i = 1, 2), taking the update of the left foot's position coordinates as an example (the same applies to the right foot), we have the following relationship:

[0087]

[0088] Therefore, the updated coordinates of the left foot are:

[0089]

[0090] The navigation error is estimated and corrected using a Kalman filter. The state vector of the filter is defined as follows:

[0091]

[0092] Where δV is the velocity error, δP is the position error, and δφ is the attitude error. To add zero bias, ε represents gyroscope drift.

[0093] The system's measurement equation is:

[0094] Z k =H k X k +V k

[0095] Among them, Z k For the measurement, H k For the measurement matrix, V k To measure noise in the system.

[0096] The position error measurement at this time can be expressed as:

[0097]

[0098] The corresponding measurement matrix is:

[0099] H k =[0 3×3 I 3×3 0 3×9 ]

[0100] Among them, I 3×3 It is a 3×3 identity matrix.

[0101] Navigation error divergence can be suppressed using the standard Kalman filtering process.

[0102] This invention proposes a pedestrian navigation method based on inter-foot distance constraints. This method sets different inter-foot distance constraint thresholds for the normal walking and running stages, thereby accurately updating the position coordinates. Furthermore, it uses a Kalman filter to suppress navigation error divergence, achieving high-precision positioning and navigation for pedestrians.

[0103] The features described and / or illustrated above for one embodiment may be used in the same or similar manner in one or more other embodiments, and / or in combination with or in lieu of features in other embodiments.

[0104] It should be emphasized that the term "including / comprises" as used herein refers to the presence of a feature, whole, step, or component, but does not exclude the presence or addition of one or more other features, wholes, steps, components, or combinations thereof.

[0105] Many features and advantages of these embodiments are apparent from this detailed description, and therefore the appended claims are intended to cover all such features and advantages of these embodiments that fall within their true spirit and scope. Furthermore, since many modifications and alterations will readily occur to those skilled in the art, the embodiments of the invention are not intended to be limited to the precise structures and operations illustrated and described, but rather to encompass all suitable modifications and equivalents falling within their scope.

[0106] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0107] The parts of this invention not described in detail are techniques known to those skilled in the art.

Claims

1. A pedestrian navigation method based on interfoot distance constraints, characterized in that, Includes the following steps: Set a first threshold for the interfoot distance constraint during normal walking; Set the distance between feet to a first threshold when one foot touches the ground during running, and the distance between feet to a second threshold when both feet are off the ground. Two micro-inertial sensors are attached to the left and right feet of a pedestrian, respectively. Data from the output of either micro-inertial sensor is collected to determine whether the movement is normal walking or running. In the running state, it is judged as either one foot touching the ground or both feet in the air. Calculate the distance between the feet at the current moment; In normal walking mode, if the distance between the feet is greater than a first threshold, the coordinates of both feet are updated based on the first threshold. When running with one foot on the ground, if the distance between the feet is greater than a first threshold, then update the coordinates of both feet based on the first threshold. When both feet are off the ground during running, if the distance between the feet is greater than the second threshold, then the coordinates of the feet are updated based on the second threshold. Kalman filtering is used to estimate and correct navigation errors; The first threshold is calculated based on the pedestrian's leg length, and the formula for calculating the first threshold is: in, For pedestrian leg length, This is the proportionality coefficient; The proportionality coefficient The determination method is as follows: Where h is the pedestrian's height; The second threshold is calculated based on the pedestrian's leg length and take-off height. The formula for calculating the second threshold is: in, For pedestrian leg length, The relative height of the foot to the ground is obtained through inertial navigation calculation.

2. The pedestrian navigation method according to claim 1, characterized in that, The method for determining motion state includes the following steps: Select a micro inertial sensor and calculate its triaxial resultant acceleration; Set a first sliding window and calculate the variance of the triaxial resultant acceleration of the micro inertial sensor in the first sliding window; If the variance is greater than the set variance threshold, the pedestrian is in a running state; otherwise, they are in a normal walking state.

3. The pedestrian navigation method according to claim 2, characterized in that, The formula for calculating the variance of the triaxial resultant acceleration is: in, The i-th triaxial resultant acceleration within the first sliding window. It is the mean of the three-axis resultant acceleration within the first sliding window, and n is the length of the first sliding window.

4. The pedestrian navigation method according to claim 1, characterized in that, The method to determine whether running involves one foot touching the ground or both feet being off the ground is as follows: Collect data from the micro-inertial sensors on both feet to determine if one foot is at zero speed. If so, the current state is one foot on the ground; otherwise, both feet are off the ground.

5. The pedestrian navigation method according to claim 4, characterized in that, The condition for satisfying the zero-velocity state is: in, The measured value of the Y-axis acceleration at time k. The resultant angular velocity of the three axes of the micro inertial sensor. , Y-axis acceleration threshold This is the angular velocity threshold.

6. The pedestrian navigation method according to claim 1, characterized in that, The method for calculating the distance between feet at the current moment is as follows: in, , , These represent the latitude, altitude, and longitude obtained from the left foot inertial navigation system at time k. , , These represent the latitude, altitude, and longitude calculated by the right foot inertial navigation system at time k, respectively. M R N Let L be the Earth's principal curvature radius and L be the local latitude.

7. The pedestrian navigation method according to claim 1, characterized in that, The method for updating the coordinates of the two feet is as follows: in, , These are the updated position coordinates of the left and right feet, respectively. , These are the position coordinates of the left and right feet obtained from the inertial navigation system calculations. , These are the first and second thresholds, respectively. This refers to the distance between the feet.

Images