A pedestrian navigation method based on RANSAC algorithm

Abstract

This invention discloses a pedestrian navigation method based on the RANSAC algorithm, relating to the field of pedestrian navigation. The method collects acceleration and angular velocity data during pedestrian movement using an inertial measurement unit (IMU). First, it calculates the sensor variance within a sliding window and dynamically adjusts the adaptive threshold for determining a stationary state. Then, it uses the RANSAC algorithm for multiple rounds of random sampling and model validation. By constructing a stationary state model, it filters interior points, retaining the model with the most interior points and determining the stationary moment based on the proportion of interior points. When stationary is determined, it triggers Zero Velocity Correction (ZUPT) and combines it with Kalman filtering to achieve navigation and positioning. The process noise covariance matrix and observation noise covariance matrix of the Kalman filter can be adaptively adjusted during operation. This invention solves the problem of traditional threshold methods being susceptible to noise and outlier interference, significantly improving the robustness of stationary state detection and the positioning accuracy of pedestrian navigation.

Description

Technical Field

[0001] This invention relates to the field of pedestrian navigation, and more particularly to a pedestrian navigation method based on the RANSAC algorithm. Background Technology

[0002] Inertial Measurement Units (IMUs) have become core sensors in pedestrian navigation due to their small size, light weight, and portability. An IMU contains an accelerometer and a gyroscope, capable of measuring the acceleration and angular velocity of a pedestrian during movement, thus achieving navigation and positioning. However, due to the inherent drift characteristics of IMUs, prolonged use of IMU integration for navigation can lead to a rapid accumulation of positioning errors, severely impacting navigation accuracy.

[0003] Zero-speed correction (ZUPT) is a key technology for solving IMU drift in pedestrian navigation. When the IMU is attached to a pedestrian's foot, the theoretical velocity of the pedestrian is zero at the moment the heel touches the ground during normal walking. This constraint can be used to calibrate the IMU's error, thereby improving navigation accuracy. Therefore, accurately determining the pedestrian's moment of stillness is a prerequisite for implementing ZUPT.

[0004] Traditional methods for determining stationary states often employ thresholding, setting criteria such as angular velocity below a certain threshold and acceleration modulus close to gravitational acceleration. However, during pedestrian movement, IMU measurements are affected by noise from normal motion, sensor malfunctions, and external shocks. Thresholding methods may misjudge non-stationary states as stationary ones, or miss true moments of stillness, causing ZUPT (Zero-Use Thresholding Test) to fail, leading to navigation errors and impacting accuracy. Furthermore, traditional thresholding methods typically use fixed thresholds, which cannot adapt to varying sensor noise levels in different motion scenarios. For example, during brief pauses after vigorous movement, sensor noise variance is high, and a fixed threshold may miss true motion; conversely, during stable standing, sensor noise variance is low, and a fixed threshold may mistake a non-stationary state for stillness.

[0005] Kalman filtering is a commonly used state estimation algorithm in pedestrian navigation. By fusing IMU state predictions and observation information, it can effectively suppress error accumulation. In zero-velocity correction scenarios, the observed value is usually the theoretical velocity of zero. However, in practical applications, due to the time-varying characteristics of observation noise, traditional Kalman filtering uses a fixed observation noise covariance matrix, which cannot be applied to the event-varying characteristics of noise, leading to a decrease in filtering accuracy during zero-velocity correction.

[0006] Therefore, this invention provides a navigation method that can reliably determine the moment a pedestrian stops and combines it with adaptive Kalman filtering to improve the accuracy and robustness of pedestrian navigation. Summary of the Invention

[0007] The purpose of this invention is to overcome the shortcomings of the traditional threshold method in pedestrian navigation for determining stationary status, and to provide a pedestrian navigation method based on the RANSAC algorithm to improve the accuracy of stationary status determination. At the same time, it combines adaptive Kalman filtering technology to further improve the positioning accuracy and robustness of pedestrian navigation.

[0008] This invention relates to a pedestrian navigation method based on the RANSAC algorithm, comprising: Step 1: completing real-time IMU data acquisition; Step 2: estimating the variance of the sensor based on the acquired acceleration and angular velocity data; Step 3: calculating an adaptive threshold based on the variance obtained in Step 2; Step 4: determining the moment of stillness during the pedestrian's movement using the RANSAC algorithm; Step 5: performing zero-velocity correction on the moment of stillness; Step 6: implementing adaptive Kalman filtering for navigation and positioning.

[0009] In step one, accelerometer data of the pedestrian's movement is collected using an inertial measurement unit (IMU). and angular velocity data ,in , This represents the number of data points.

[0010] In step two, the variance within a local time window is calculated for the collected accelerometer and angular velocity data to obtain the acceleration variance. angular velocity variance ,in:

[0011]

[0012] In the formula, W is half the length and width of the sliding window. and These are the mean values ​​of the acceleration modulus and angular velocity modulus within the window, respectively.

[0013] In step three, the static state judgment threshold is dynamically adjusted based on the variance obtained in step two. The calculation formula is as follows:

[0014]

[0015] In the formula, and Based on the threshold, This is the variance influence factor, with a value range of [value range missing]. .

[0016] In step four, the stationary time needs to be determined using the RANSAC algorithm. The RANSAC algorithm process is as follows: randomly select from the IMU data within the acquired window. One sample point, To minimize the number of points required to fit the static state model, therefore, we take... Based on the constructed static state model and the adaptive threshold obtained in step three, it is determined whether the selected sample point is an interior point. The formula for determining an interior point is:

[0017] In the formula, This is the model value of the acceleration modulus calculated based on random sampling points.

[0018] Iterate through all IMU data points within the window using the current stationary state model, counting the number of inliers that satisfy the constraints. Repeat the above process of random sampling, inlier determination, and inlier counting, setting the number of iterations, and retain the model with the most inliers. If the proportion of inliers in the model with the most inliers exceeds a preset inlier proportion threshold, then the time period corresponding to this IMU data segment is determined to be the pedestrian stationary moment.

[0019] In step five, the moment when the pedestrian is stationary is obtained through step four, triggering zero-velocity correction, and the velocity of 0 is introduced into the Kalman filter as the observation value.

[0020] Step six implements adaptive Kalman filter navigation and positioning. First, the system's state variables are defined as follows:

[0021] in, Misalignment angle For speed error, For positional error, To achieve zero bias in the gyroscope, This is for zero bias of the accelerometer.

[0022] The state equation of the system is:

[0023] in, Here is the state transition matrix. Here is the noise gain matrix. This is process noise.

[0024] The observation equation is as follows:

[0025] in, For the velocity observation vector to satisfy the zero-velocity correction condition, For the observation matrix, To observe noise.

[0026] Dynamically adjust the accelerometer variance in the Kalman filter for:

[0027] in, This is the proportionality coefficient. The variance of acceleration within the current time window. Based on the observation noise covariance matrix.

[0028] Process noise covariance Adjust as follows:

[0029] in, This is the proportionality coefficient. The variance of angular velocity degrees within the current time window. It is the noise covariance matrix of the basic process.

[0030] The Kalman filter iterates according to the formula, and the prediction steps are as follows:

[0031]

[0032] The update steps are as follows:

[0033]

[0034]

[0035] Based on the above processing steps, sensor data during pedestrian movement can be collected in real time, enabling static detection of pedestrians. After correction by Kalman filtering, navigation information of pedestrians can be output in real time.

[0036] Compared with the prior art, the advantages of the present invention are: 1. The present invention designs a static pedestrian detection based on the RANSAC algorithm. By utilizing the noise resistance and outlier capability of the RANSAC algorithm, a static state model is constructed and IMU data is processed. It can accurately determine the static time of pedestrians from noise and outlier data. Compared with the traditional threshold method, it can still accurately identify the static state under the condition of outlier interference.

[0037] 2. This invention uses a sliding window to calculate the local variance of sensor data within the window, and adaptively adjusts the static state judgment threshold and the variance matrix of the Kalman filter, thus adapting to changes in sensor noise levels under different motion scenarios.

[0038] 3. This invention provides an accurate triggering time for zero-speed correction in pedestrian navigation, effectively calibrates the drift error of the IMU, and can improve the accuracy of pedestrian navigation. Attached Figure Description

[0039] Appendix Figure 1 This is a flowchart illustrating the pedestrian navigation method based on the RANSAC algorithm of this invention. (Attached) Figure 2 This is a flowchart of the RANSAC algorithm in this invention. Detailed Implementation

[0040] To make the technical problems, technical solutions, and beneficial effects of this invention clearer, the invention will be described in detail below with reference to the accompanying drawings and embodiments. It should be noted that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0041] Figure 1 This is a flowchart illustrating the pedestrian navigation method based on the RANSAC algorithm of this invention. In this invention, the coordinate systems are first defined as follows: the vehicle coordinate system is "front right-lower," and the navigation coordinate system is "northeast-ground." The calculation method includes: Step 1: Attach the IMU (Import Unit) to the shoe to collect data. The pedestrian begins to walk, and the accelerometer output data is continuously collected during the walking process. and angular velocity output data The sampling frequency is set to 100Hz.

[0042] Step 2: Set the value W, which is half the width and length of the sliding window, to 50, corresponding to a data acquisition time of 0.5 seconds. Once the window is full, begin calculations. Subsequent windows slide in 0.1-second increments. Calculate the variance within each time window to obtain the acceleration variance. angular velocity variance ,in:

[0043]

[0044] In the formula, W is half the length and width of the sliding window. and These are the mean values ​​of the acceleration modulus and angular velocity modulus within the window, respectively.

[0045] The third step is to dynamically adjust the static state judgment threshold based on the variance obtained in step two. The calculation formula is as follows:

[0046]

[0047] In the formula, and Based on the base threshold, take the base threshold. It is 0.5 , It is 0.5 , This is the variance impact factor, with a value of 0.5.

[0048] The fourth step is to determine the stationary time using the RANSAC algorithm. The RANSAC algorithm process is as follows: randomly select from the IMU data within the acquired window. A number of sample points. Based on the constructed static state model and the adaptive threshold obtained in step three, determine whether the selected sample points are interior points. The formula for determining interior points is:

[0049] In the formula, This is the model value of the acceleration modulus calculated based on random sampling points.

[0050] Iterate through all IMU data points within the window using the current stationary state model, counting the number of inliers that satisfy the constraints. Repeat the above process of random sampling, inlier determination, and inlier counting, setting the iteration count to 100 times, and retain the model with the most inliers. If the proportion of inliers in the model with the most inliers exceeds 80%, then the time period corresponding to this IMU data segment is determined to be the pedestrian's stationary moment.

[0051] Fifth, when the pedestrian's stationary moment is determined in the fourth step, zero-velocity correction is triggered, and the velocity of 0 is introduced into the Kalman filter as the observation value.

[0052] Step 6: Implement adaptive Kalman filter navigation and positioning. First, define the system's state variables as:

[0053] in, Misalignment angle For speed error, For positional error, To achieve zero bias in the gyroscope, This is for zero bias of the accelerometer.

[0054] The state equation of the system is:

[0055] in, Here is the state transition matrix. Here is the noise gain matrix. This is process noise.

[0056] The observation equation is as follows:

[0057] in, For the velocity observation vector to satisfy the zero-velocity correction condition, For the observation matrix, To observe noise.

[0058] Dynamically adjust the accelerometer variance in the Kalman filter for:

[0059] in, This is the proportionality coefficient. The variance of acceleration within the current time window. Based on the observation noise covariance matrix.

[0060] Process noise covariance Adjust as follows:

[0061] in, This is the proportionality coefficient. The variance of angular velocity degrees within the current time window. It is the noise covariance matrix of the basic process.

[0062] The Kalman filter iterates according to the formula, and the prediction steps are as follows:

[0063]

[0064] The update steps are as follows:

[0065]

[0066]

[0067] The above processing method enables real-time acquisition of sensor data during pedestrian movement, static detection of pedestrians, and real-time output of pedestrian navigation information through Kalman filtering correction.

[0068] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the principles and spirit of the present invention are included within the protection scope of the present invention.

Claims

1. A pedestrian navigation method based on the RANSAC algorithm, characterized in that, include: Step 101: Acceleration and angular velocity data of the pedestrian during movement are collected using an inertial measurement unit; Step 102: For the collected sensor data, set a sliding window, calculate the variance within the sliding window, and obtain the acceleration variance and angular velocity variance; Step 103: Based on the obtained variance, dynamically adjust the judgment threshold for the static state; Step 104: Use the RANSAC algorithm, combined with an adaptive threshold, to determine whether the pedestrian is stationary; Step 105: When it is determined that the pedestrian is stationary, zero-velocity correction is triggered, and the IMU error is calibrated using the constraint of zero velocity. Step 106: Based on the obtained acceleration and angular velocity variances, adaptively adjust the noise covariance matrix of the Kalman filter to perform pedestrian navigation calculations.

2. The pedestrian navigation method based on the RANSAC algorithm according to claim 1, characterized in that, The first step is to collect acceleration and angular velocity data of the pedestrian's feet during the pedestrian's movement and output them in real time.

3. The pedestrian navigation method based on the RANSAC algorithm according to claim 1, characterized in that, The second step involves calculating the variance within a local time window based on the collected accelerometer and angular velocity data, thus obtaining the acceleration variance. angular velocity variance in: In the formula, W is half the length and width of the sliding window. and These are the mean values ​​of the acceleration modulus and angular velocity modulus within the window, respectively.

4. The pedestrian navigation method based on the RANSAC algorithm according to claim 1, characterized in that, The third step involves dynamically adjusting the static state judgment threshold based on the variance obtained in the second step. The calculation formula is as follows: In the formula, δ a0 and δ ω0 The base threshold is α, which is the variance influence factor and has a value range of [0,1].

5. The pedestrian navigation method based on the RANSAC algorithm according to claim 1, characterized in that, The fourth step requires determining the stationary time using the RANSAC algorithm. The RANSAC algorithm process is as follows: l sample points are randomly selected from the IMU data within the acquired window, where l is the minimum number of points required to fit the stationary state model. Based on the constructed stationary state model and the adaptive threshold obtained in the third step, it is determined whether the selected sample points are interior points. The formula for determining interior points is: In the formula, g model This is the model value of the acceleration modulus calculated based on random sampling points. Iterate through all IMU data points within the window using the current stationary state model, counting the number of inliers that satisfy the constraints. Repeat the above process of random sampling, inlier determination, and inlier counting, setting the number of iterations, and retain the model with the most inliers. If the proportion of inliers in the model with the most inliers exceeds a preset inlier proportion threshold, then the time period corresponding to this IMU data segment is determined to be the pedestrian stationary moment.

6. The pedestrian navigation method based on the RANSAC algorithm according to claim 1, characterized in that, The fifth step involves obtaining the pedestrian's stationary moment through step four, triggering zero-velocity correction, and introducing a velocity of 0 as the observation value into the Kalman filter.

7. The pedestrian navigation method based on the RANSAC algorithm according to claim 1, characterized in that, The sixth step is to implement adaptive Kalman filter navigation and positioning. First, define the system's state variables as follows: X=[φ δv δp b g b a ] T Where φ is the misalignment angle, δv is the velocity error, δp is the position error, and b g For zero bias of the gyroscope, b a This is for zero bias of the accelerometer. The state equation of the system is: X k+1 =F k X k +G k ω k Among them, F k Let G be the state transition matrix. k Let ω be the noise gain matrix. k This is process noise. The observation equation is as follows: Among them, Z k For the velocity observation vector to satisfy the zero-velocity correction condition, Z k =[O3 I3 O3 O3 O3] is the observation matrix, v k To observe noise. In the Kalman filter, R is dynamically adjusted based on the accelerometer variance. k for: Where β is the proportionality coefficient. R0 represents the acceleration variance within the current time window, and R0 is the basic observation noise covariance matrix. Process noise covariance Q k Adjust as follows: Where γ is the proportionality coefficient. Let Q0 be the variance of the angular velocity within the current time window, and let Q0 be the covariance matrix of the basic process noise. The Kalman filter iterates according to the formula, and the prediction steps are as follows: The update steps are as follows: P k / k =(I-K k H k )P k / k-1 Based on the above processing steps, sensor data during pedestrian movement can be collected in real time, enabling static detection of pedestrians. After correction by Kalman filtering, navigation information of pedestrians can be output in real time.

Images