An intelligent iron shoe precise positioning algorithm based on fusion of RTK and inertial navigation

Abstract

The application discloses an intelligent iron shoe precise positioning algorithm based on fusion of RTK and inertial navigation, and particularly relates to the technical field of GNSS and inertial navigation system fusion; the algorithm constructs a double-difference observation model based on GNSS pseudo-range and carrier phase observation values, combines IMU acceleration and angular velocity data, and realizes high-precision fusion positioning through extended Kalman filtering; a direction scattering factor DSF and a gait stability factor GSI are introduced to dynamically evaluate GNSS signal environment and user motion state, adjust an observation noise covariance matrix, and realize adaptive optimization of weights; the positioning credibility score is calculated according to the DSF and the GSI, the positioning state level is divided, and when the score is lower than a threshold value, an inertial navigation priority mode or an abnormal alarm is triggered; the application improves the stability and safety of the positioning system in a complex environment, and has strong engineering application value.

Description

Technical Field

[0001] This invention relates to the field of GNSS and inertial navigation system fusion technology, specifically to an intelligent shoe precise positioning algorithm that integrates RTK and inertial navigation. Background Technology

[0002] Smart steel shoe positioning refers to a high-tech wearable device used in specialized industries (such as mining, steel, or railway operations). It embeds a positioning system (such as GPS, BeiDou, UWB, etc.) into the steel shoes (protective shoes) worn by workers, enabling real-time and accurate tracking of personnel location. This technology can be used for safety monitoring, path recording, personnel scheduling, and other scenarios, improving operational safety and management efficiency.

[0003] In RTK and inertial navigation fusion positioning systems, GNSS signals are susceptible to obstruction and multipath interference in complex environments (such as underground mines and steel structure factories), leading to the accumulation of carrier phase observation errors. When the system incorrectly determines the integer ambiguity solution as "fixed," although it still outputs a positioning result with centimeter-level "accuracy," the actual position may deviate by several meters or even more. This type of "false accuracy" problem usually does not trigger abnormal alarms and can easily mislead system users, especially in high-risk applications such as personnel positioning and security monitoring, potentially causing serious consequences. Summary of the Invention

[0004] The purpose of this invention is to provide an intelligent shoe precise positioning algorithm that integrates RTK and inertial navigation to address the shortcomings of the prior art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a precise positioning algorithm for intelligent track shoes that integrates RTK and inertial navigation, comprising:

[0006] Collect GNSS pseudorange observations, carrier phase observations, and IMU acceleration and angular velocity data, and perform timestamp alignment and preprocessing on them;

[0007] Based on the GNSS pseudorange observations, the real-time dynamic differential method is used to calculate the double difference observations of pseudorange and carrier phase, and combined with IMU data, the preliminary RTK-INS fusion positioning is achieved through extended Kalman filtering to obtain the preliminary position and attitude solutions.

[0008] A directional distribution matrix is ​​constructed based on the unit vectors of each satellite direction in the acquired GNSS signal, its eigenvalues ​​are calculated, and the directional scattering factor (DSF) is determined based on the ratio of the maximum eigenvalue to the total eigenvalues.

[0009] Based on the collected IMU acceleration data, the user's gait cycle characteristics are extracted. By comparing with historical gait templates, the cycle variance and correlation score of the current gait are calculated, and the gait stability factor GSI is constructed accordingly.

[0010] The DSF and GSI are input as dynamically corrected weight parameters into the extended Kalman filter to adjust the observation noise covariance matrix, dynamically change the weight ratio in the RTK and inertial navigation data fusion process, and generate a confidence-weighted positioning result.

[0011] Based on the output positioning results and their corresponding DSF and GSI indices, the positioning reliability score is calculated and divided into multiple levels. When the reliability is lower than the preset threshold, the system enters the inertial navigation priority mode or triggers an alarm mechanism to indicate abnormal positioning quality.

[0012] Preferably, the process of aligning and preprocessing the data with timestamps includes:

[0013] Iterate through the timestamps of each GNSS observation point;

[0014] Find the nearest timestamp in the IMU data;

[0015] If the time difference is less than the threshold Δt, then it is matched as the same time.

[0016] If the difference exceeds the threshold Δt, then linear interpolation is performed to calculate the matching value of the IMU;

[0017] Construct an observation matrix on a unified time axis for GNSS and IMU data.

[0018] Preferably, the step of calculating the double-difference observation of pseudorange and carrier phase based on the GNSS pseudorange observations using a real-time dynamic difference method includes:

[0019] Receive dual-frequency pseudorange and carrier phase observation data from GNSS base stations and mobile terminals;

[0020] Based on the same time epoch, construct pseudorange single-difference and double-difference observation models between satellite and receiver;

[0021] Common error terms, including satellite clock error, receiver clock error, and atmospheric delay error, are eliminated by constructing double-difference observations.

[0022] The double-difference pseudorange and carrier phase observations are obtained for ambiguity fixing and position calculation.

[0023] Preferably, the ambiguity is fixed, including:

[0024] Construct an integer ambiguity resolution model for double-difference carrier phase observations;

[0025] The least squares ambiguity solution algorithm is used to make a preliminary estimate of the integer ambiguity;

[0026] Integer search of the estimated value based on fuzziness confidence interval;

[0027] When the ambiguity solvability threshold condition is met, the ambiguity is fixed as an integer solution, and the preliminary positioning coordinates are output.

[0028] Preferably, the preliminary RTK-INS fusion localization achieved by combining IMU data with extended Kalman filtering to obtain preliminary position and attitude solutions includes:

[0029] RTK positioning results are used as system state observation inputs, and IMU acceleration and angular velocity are used as prediction inputs;

[0030] Construct the system state transition equation and observation equation, and establish a state vector that includes position, velocity, attitude and sensor bias;

[0031] The prediction and update process is executed iteratively, and the optimal state solution after fusion is estimated in real time.

[0032] The output contains a fused solution including position coordinates, heading angle, and velocity vector.

[0033] Preferably, the calculation of the directional scattering factor DSF includes:

[0034] Obtain the direction vectors of the n available GNSS satellites in the current epoch, normalize them, and form a set of direction vectors;

[0035] All unit direction vectors are used to form a direction observation matrix V, with dimensions of n rows and 3 columns;

[0036] Calculate the covariance matrix S of the direction observation matrix V, where S is equal to the product of the transpose of V and V.

[0037] Perform eigenvalue decomposition on matrix S to obtain three real eigenvalues, denoted as , ... ;

[0038] DSF equals 1 minus the ratio of the largest eigenvalue to the total eigenvalues.

[0039] Preferably, the calculation of the gait stability factor GSI includes:

[0040] Based on the collected IMU triaxial acceleration data, the vertical acceleration signal sequence within the user's continuous walking cycle is extracted;

[0041] The peak detection method is used to identify key step points within the gait cycle, including the foot strike point and the departure point;

[0042] Calculate the time interval of several consecutive gait cycles to obtain periodic sequence data;

[0043] The periodic variance is calculated from the periodic sequence;

[0044] Standard gait templates are extracted from historical normal gait data to construct reference periodic waveforms;

[0045] The time alignment distance between the current gait signal and the template is calculated using a dynamic time warping algorithm.

[0046] The variance of the gait cycle and the correlation score of the dynamic time warping algorithm are used as inputs;

[0047] The gait stability factor GSI is calculated by combining the negative periodic variance exponent of e with the correlation score of the dynamic time warping algorithm.

[0048] Preferably, the DSF and GSI are input as dynamically adjusted weight parameters into the extended Kalman filter to adjust the observation noise covariance matrix, including:

[0049] In each fusion epoch, the directional scattering factor DSF and the gait stability factor GSI are calculated respectively.

[0050] The DSF and GSI are both used as weight correction factors W and input to the observation noise covariance matrix adjustment module.

[0051] The expression for calculating the weight correction factor W is: Where α and β are adjustable weighted sensitivity coefficients;

[0052] In the update step of the extended Kalman filter, the original value of the covariance matrix R of the GNSS related observations is multiplied by the correction factor W to obtain the corrected observation noise covariance matrix.

[0053] The technical effects and advantages provided by the present invention in the above technical solution are as follows:

[0054] 1. This invention constructs a two-dimensional positioning reliability assessment mechanism oriented towards both environment and motion state by introducing the Directional Scattering Factor (DSF) and Gait Stability Factor (GSI), breaking the limitation of traditional RTK-INS systems that rely solely on the state of GNSS observation data for fusion control. Through quantitative modeling of the uniformity of GNSS signal directional distribution and IMU motion stability, real-time perception and response to positioning quality are achieved. This enables the system to dynamically adjust the fusion weights of GNSS and IMU under abnormal conditions such as occlusion, multipath interference, or unstable gait, effectively suppressing error propagation and positioning jumps.

[0055] 2. Based on the positioning reliability scoring, this invention further constructs a multi-level state division mechanism and an adaptive fusion control strategy. It can automatically switch to inertial navigation priority mode or trigger an alarm mechanism in low-reliability states, thereby improving the system's safety and robustness in high-risk operating environments. This technical solution features lightweight structure, high computational efficiency, and strong environmental adaptability, making it suitable for complex GNSS environments such as underground mines, tunnel construction, and industrial plants, and possessing significant engineering practical value. Attached Figure Description

[0056] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this invention. For those skilled in the art, other drawings can be obtained based on these drawings.

[0057] Figure 1 This is a flowchart of the algorithm structure of the present invention. Detailed Implementation

[0058] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0059] For examples, please refer to Figure 1 As shown in this embodiment, a precise positioning algorithm for intelligent track shoes that integrates RTK and inertial navigation includes:

[0060] Collect GNSS pseudorange observations, carrier phase observations, and IMU acceleration and angular velocity data, and perform timestamp alignment and preprocessing on them;

[0061] Based on the GNSS pseudorange observations, the real-time dynamic differential method is used to calculate the double difference observations of pseudorange and carrier phase, and combined with IMU data, the preliminary RTK-INS fusion positioning is achieved through extended Kalman filtering to obtain the preliminary position and attitude solutions.

[0062] A directional distribution matrix is ​​constructed based on the unit vectors of each satellite direction in the acquired GNSS signal, its eigenvalues ​​are calculated, and the directional scattering factor (DSF) is determined based on the ratio of the maximum eigenvalue to the total eigenvalues.

[0063] Based on the collected IMU acceleration data, the user's gait cycle characteristics are extracted. By comparing with historical gait templates, the cycle variance and correlation score of the current gait are calculated, and the gait stability factor GSI is constructed accordingly.

[0064] The DSF and GSI are input as dynamically corrected weight parameters into the extended Kalman filter to adjust the observation noise covariance matrix, dynamically change the weight ratio in the RTK and inertial navigation data fusion process, and generate a confidence-weighted positioning result.

[0065] Based on the output positioning results and their corresponding DSF and GSI indices, the positioning reliability score is calculated and divided into multiple levels. When the reliability is lower than the preset threshold, the system enters the inertial navigation priority mode or triggers an alarm mechanism to indicate abnormal positioning quality.

[0066] In this invention, the GNSS data acquisition module is located within the foot structure of the smart shoe, employing a high-performance dual-frequency GNSS receiver module that supports receiving signals from both the L1 and L2 frequency bands. To meet the requirement of centimeter-level positioning accuracy, the GNSS module must simultaneously lock onto signals from at least four satellites. The system acquires pseudorange and carrier phase observations at a frequency of 1 to 10 times per second and formats and stores them in RINEX format (i.e., receiver-independent exchange format) for subsequent differential positioning processing.

[0067] GNSS pseudorange refers to the distance measurement obtained by multiplying the propagation time between the receiver and the satellite by the speed of light. Carrier phase measurement, on the other hand, is obtained by tracking the phase changes of the satellite signal carrier wave, and its accuracy is far higher than pseudorange observation, typically reaching the millimeter level. To eliminate systematic errors and satellite clock bias, this system constructs a double-difference observation model based on pseudorange and carrier phase: first, the difference between two receivers is calculated for the same satellite (called "single difference"), and then the single difference between the two satellites is subtracted again (i.e., "double difference") to further suppress common error terms.

[0068] The above operations yield pseudorange double-difference observations and carrier phase double-difference observations, which serve as the input basis for subsequent RTK solutions. This step not only improves the stability of the solution but also reduces the dependence on absolute time synchronization accuracy.

[0069] An inertial measurement unit (IMU) integrates a triaxial accelerometer, a triaxial gyroscope, and a triaxial magnetometer to collect dynamic motion characteristics of the wearer's feet in space. To capture the complete gait cycle and minute posture changes, the IMU sampling frequency is set to at least 100 Hz, and can be extended to 200 Hz or higher as needed. The sampled data includes triaxial linear acceleration, triaxial angular velocity, and geomagnetic direction data.

[0070] IMU signals are susceptible to noise from footsteps, metallic interference, and other sources. To suppress high-frequency noise and preserve accurate motion signals, a low-pass filter is embedded in the acquisition module. A finite impulse response (FIR) filter structure is used, with a cutoff frequency of 20 Hz and a filter order of 50, to ensure that complete gait data is preserved while removing interference signals.

[0071] The filtered IMU data is used for subsequent gait feature extraction, pose estimation, and RTK fusion processing.

[0072] Since the GNSS module and IMU module operate at different sampling frequencies and the time reference may come from different clock sources, the collected observation data needs to be time-aligned to ensure the accuracy of multi-source data synchronization and fusion.

[0073] To achieve high-precision time alignment, the system of this invention is equipped with a unified local real-time clock (RTC) module to provide a unified timestamp reference for both the GNSS and IMU modules. GNSS observation data is sampled in seconds, while IMU data is sampled in milliseconds. Since the sampling times of the two may not be consistent, a nearest neighbor interpolation algorithm is used for resampling.

[0074] The specific steps are as follows:

[0075] Iterate through the timestamps of each GNSS observation point;

[0076] Find the nearest timestamp in the IMU data;

[0077] If the time difference is less than the threshold Δt (5 milliseconds in this system), then it is matched as the same time.

[0078] If the difference exceeds the threshold, linear interpolation is performed to calculate the matching value of the IMU;

[0079] Construct an observation matrix on a unified time axis for GNSS and IMU data.

[0080] The above method enables multi-frequency collaborative synchronization of GNSS and IMU data, ensuring that errors are minimized in the subsequent fusion model.

[0081] After data synchronization is completed, in order to improve the computational efficiency and robustness of filtering and fusion algorithms, normalization, standardization and noise removal processing of GNSS and IMU data are also required.

[0082] GNSS data preprocessing includes:

[0083] Perform integrity checks to determine if there are jumps or loss in pseudorange and phase. If a satellite's data is missing for more than two consecutive epochs, then that data will be removed.

[0084] Implement a gross error removal algorithm: adopt the 3σ principle (that is, based on the historical mean of the current epoch, if the deviation of the observed value exceeds 3 times the standard deviation, it is judged as a gross error and removed);

[0085] The observation data is transformed from the receiver's local coordinate system to the global geocentric coordinate system (ECEF) to enable unified modeling with IMU data.

[0086] IMU data preprocessing includes:

[0087] Perform coordinate transformation on the original acceleration and angular velocity signals to project the data in the sensor coordinate system to the geographic reference coordinate system (ENU, East-North-Sky coordinate system).

[0088] The dimensions of the outputs from different sensors are normalized to eliminate numerical scale bias caused by different units or dimensions.

[0089] Compensation is applied to the gravitational acceleration term so that the acceleration reflects only the motion process rather than the static bias.

[0090] After preprocessing, all observation data are input into the subsequent RTK-INS fusion filter and weight correction model in a unified format to ensure the continuity of the algorithm execution process and the stability of the computation.

[0091] In this invention, the real-time dynamic differential processing of GNSS data is based on dual-frequency observations and encompasses the following key technical steps:

[0092] Both the GNSS base station and mobile terminal are equipped with dual-frequency receivers to receive pseudorange and carrier phase observations from multiple GNSS satellites (such as GPS and BeiDou) in two frequency bands (such as L1 and L2). The sampling frequency can be set between 1 Hz and 10 Hz to ensure real-time performance.

[0093] Pseudorange observations are distance information calculated by measuring the propagation time of satellite signals, and are measured in meters; carrier phase observations are the phase difference between the signal between the receiver and the satellite, measured in cycles, and are more accurate, making them suitable for high-precision positioning.

[0094] To suppress systematic errors, the system first performs a difference analysis on the observation data of the same satellite between the base station and the mobile terminal based on the same time epoch, and obtains a single-difference observation, which is used to eliminate receiver clock errors.

[0095] Subsequently, the single-difference observations are differentially analyzed between different satellites to obtain double-difference observations, thereby eliminating common error terms such as satellite clock errors and atmospheric delay errors.

[0096] In mathematical terms, pseudorange double-difference observations can be described as:

[0097] Double-difference pseudorange = (Mobile terminal - Base station) pseudorange difference to satellite A - (Mobile terminal - Base station) pseudorange difference to satellite B.

[0098] Similarly, the carrier phase double difference observation is structurally the same, but it uses carrier phase observations.

[0099] The above double-difference processing can effectively eliminate the following error terms:

[0100] Satellite clock bias;

[0101] Receiver clock bias;

[0102] Most of the effects of ionospheric delay and tropospheric delay;

[0103] Partial components of satellite orbital error.

[0104] The processed double-difference pseudorange observations and double-difference carrier phase observations serve as the core inputs for high-precision RTK solution, providing a stable observation data source for ambiguity fixing and fusion solution.

[0105] In RTK systems, carrier phase observations contain integer ambiguity terms. Failure to accurately resolve these integer terms will lead to significant positioning errors. Therefore, this invention employs the following steps to fix the ambiguity:

[0106] Using double-difference carrier phase observations as input, a linear observation equation is constructed, and integer ambiguity is introduced into the model as a parameter to be estimated. The model estimates the ambiguity solution by minimizing the sum of squared residuals.

[0107] The least squares method is used to estimate the floating-point ambiguity, resulting in a non-integer ambiguity vector. This estimate has the smallest variance in a statistical sense and is used as the initial solution for the subsequent integer search.

[0108] The LAMBDA algorithm is used to convert floating-point ambiguities to integers. This algorithm consists of three sub-steps: transforming the ambiguity covariance matrix, orthogonal decoupling, and integer search, which can efficiently obtain integer solutions.

[0109] A solvability threshold of P is set, typically 0.99. When the confidence score (i.e., the ratio test index of ambiguity solutions) corresponding to integer solutions is higher than this threshold, the ambiguity is considered to be fixed as an integer; otherwise, floating-point solutions are maintained.

[0110] If the fixation is successful, the high-precision positioning result after the ambiguity is fixed will be output for use by the subsequent EKF fusion module.

[0111] To achieve real-time fusion of GNSS and inertial navigation, this invention employs the Extended Kalman Filter (EKF) algorithm for state estimation. The specific steps are as follows:

[0112] This system uses a 15-dimensional state vector, specifically including:

[0113] 3D position coordinates (x, y, z);

[0114] 3D velocity vector (vx, vy, vz);

[0115] 3D attitude angles (roll, pitch, yaw);

[0116] 3D accelerometer zero bias;

[0117] 3D gyroscope with zero bias.

[0118] This vector can be further extended to higher dimensions depending on the application scenarios.

[0119] The acceleration and angular velocity provided by the inertial measurement unit are used as the system prediction inputs, and the relationship between the state changes over continuous time is derived based on the navigation mechanics model as the prediction model.

[0120] The state transition equations include:

[0121] Position changes with the integral of velocity;

[0122] Speed ​​is affected by acceleration;

[0123] Attitude changes with angular velocity (usually represented using quaternions or Euler angles);

[0124] The zero-partial term is modeled as a first-order Gaussian Markov process.

[0125] The 3D position output by RTK is used as the observation input and compared with the state prediction value to construct the observation residual.

[0126] The filtering process consists of two main stages:

[0127] Prediction phase: The state transition equation is used to predict the state and covariance at the next time step;

[0128] Update phase: Introduce RTK observations, calculate Kalman gain, update the state vector, and correct prediction errors.

[0129] The Kalman gain used in the filtering formula is calculated from the state prediction covariance matrix and the observation noise covariance matrix, ensuring that the state update minimizes the mean square error.

[0130] The final output includes:

[0131] Three-dimensional location coordinates (latitude and longitude or ECEF format);

[0132] Three-dimensional velocity vector;

[0133] Attitude angle information (yaw angle, pitch angle, roll angle);

[0134] Optionally, the state covariance can be output to represent the confidence level.

[0135] In real-world scenarios, such as urban canyons, mine shafts, and steel-structured factory buildings, GNSS signals are prone to obstruction and multipath reflection, resulting in uneven satellite orientation distribution, and in severe cases, even concentration in a single location. In such environments, even if GNSS receives a sufficient number of satellites, the positioning solution may still contain significant errors.

[0136] Traditional GNSS-RTK algorithms often use the number of satellites or PDOP (Position Precision Factor) as the basis for environmental judgment, but these indicators cannot accurately reflect the uniformity of spatial directional distribution. To address this, this invention innovatively introduces the "Directional Scattering Factor (DSF)" indicator. By constructing a directional unit vector distribution matrix and extracting its eigenvalues, it achieves quantitative modeling of the spatial distribution structure of GNSS signals and uses it to assist in noise modeling and weight allocation in the fusion algorithm.

[0137] In each positioning epoch, the GNSS receiving module can acquire the orientation information of multiple satellites, specifically including the spatial coordinates of the satellites and the position coordinates of the local receiver. Assuming that there are n visible satellites at the current moment, each satellite i corresponds to a spatial orientation vector, denoted as Vi.

[0138] Each satellite orientation vector Vi is defined as the difference vector between the satellite's spatial position in the geocentric geofixed coordinate system (ECEF) and the receiver's position.

[0139] To unify the dimensions among different satellites and focus on directional distribution characteristics, each directional vector Vi needs to be normalized to obtain a unit directional vector Ui. Specifically, the Vi vector is divided by its magnitude to ensure that all directional vectors have a length of 1, thus preserving their spatial directional characteristics.

[0140] Ultimately, a set of unit direction vectors {U1, U2, ..., Un} is formed, where each Ui is a three-dimensional vector, which together form an n-row, 3-column direction observation matrix V.

[0141] The direction observation matrix V is an n×3 real matrix. Transposing it and multiplying it by itself yields a 3×3 covariance distribution matrix S, expressed as: ;in, denoted by , × represents the transpose of matrix V, and × represents matrix multiplication. This matrix S describes the concentration and principal direction of the satellite signal direction vector in three-dimensional space.

[0142] Eigenvalue decomposition of matrix S yields three real eigenvalues, denoted as , ... Where λ_max represents the maximum value among the three, and λ_sum represents the sum of the three, that is:

[0143] ;

[0144] ;

[0145] The essence of DSF is a quantitative modeling of the uniformity of GNSS direction vector distribution and the bias of the principal direction, which is defined as follows: Theoretically, this indicator ranges between 0 and 1.

[0146] When the DSF approaches 1, it indicates that all direction vectors are relatively evenly distributed in three-dimensional space, with no obvious principal direction;

[0147] When the DSF approaches 0, it indicates that the direction vectors are highly concentrated in a certain direction or plane, indicating a serious risk of occlusion or multipath propagation.

[0148] Therefore, DSF can be used as a reflection of the spatial integrity of GNSS signals. Its physical meaning is clear and its calculation process is highly feasible.

[0149] The system collects triaxial acceleration data in real time through an inertial measurement unit (IMU) integrated inside the smart shoe. The sampling frequency is set to 100 Hz to ensure that fine-grained changes in gait characteristics can be captured. The triaxial acceleration data are denoted as Ax, Ay, and Az, corresponding to the forward, lateral, and vertical directions, respectively.

[0150] To analyze the periodic characteristics of gait, the vertical acceleration Az is preferred because it exhibits obvious periodic fluctuations during human walking, especially with peak changes during the landing and takeoff phases.

[0151] To remove high-frequency interference and background fluctuations, the system performs bandpass filtering on the Az signal. The filter passband range is set to 0.5 to 3 Hz to match the natural human cadence (60–180 steps / minute). The filter type can be a fifth-order Butterworth filter.

[0152] The filtered vertical acceleration signal exhibits an approximately sinusoidal waveform. To extract the gait period, the system employs a peak detection algorithm, the steps of which are as follows:

[0153] The minimum peak interval is set to 0.5 seconds to prevent false detection of multiple small peaks within the same period;

[0154] Set the dynamic threshold to the signal mean plus the standard deviation to eliminate false peaks caused by noise;

[0155] Positive and negative peaks are detected, corresponding to toe-off and heel-strike events, respectively.

[0156] The time interval between two consecutive peaks of the same type is defined as a gait cycle.

[0157] Repeat this process to obtain nearly N gait cycle sequences. (For example, N is 5).

[0158] By calculating the variance σ of the sequence 2 ,Right now: , where μ is the mean of the gait period. This value is used to quantify the rhythmic consistency of the current gait. The smaller the value, the more stable the gait and the higher the reliability of the inertial navigation output.

[0159] To further improve the modeling accuracy of gait features, this invention introduces a waveform similarity index and uses the Dynamic Time Warping (DTW) algorithm to calculate the similarity between the current gait waveform and the standard gait template.

[0160] Collect a large amount of historical normal gait data (such as laboratory sampling or individual modeling in the early stages of wearing the device).

[0161] Normalize and align multiple gait cycles;

[0162] The average waveform sequence is obtained as the individual standard gait template S0.

[0163] The currently observed gait sequence S1 has been normalized.

[0164] The DTW algorithm constructs a cost matrix D(i, j), where each element represents The Euclidean distance between the i-th point in S1 and the j-th point in S1;

[0165] The shortest path is searched using dynamic programming, and the cumulative minimum matching distance D_total is calculated.

[0166] Converted to a matching relevance score R, defined as: The closer R is to 1, the higher the waveform matching degree, and the better the consistency between the current gait and the standard template.

[0167] The variance σ of the gait cycle 2 In conjunction with the DTW matching score R, the gait stability factor GSI is defined as: Where: e is the base of the natural logarithm; σ 2It is the variance of the gait cycle sequence, reflecting the temporal consistency; R is the DTW matching correlation score, reflecting the morphological similarity; the value range of GSI is between 0 and 1, and the higher the value, the more stable the gait and the more reliable the inertial navigation data.

[0168] To further improve the system stability, the present invention introduces a gait stability threshold (the recommended value is 0.6) as the judgment basis for fusion weight adjustment:

[0169] If GSI ≥ T2, the system determines that the current gait is stable and maintains the existing IMU data weight;

[0170] If GSI < T2, the system reduces the IMU weight or activates the inertial navigation drift correction module to prevent the accumulation of sensor errors from affecting the final positioning.

[0171] DSF is used to characterize the spatial distribution balance of GNSS visible satellites. When its value is close to 1, it indicates that the signal directions are evenly distributed and the environment is excellent; when the value is close to 0, it means that the directions are concentrated, there are obstructions or multipath effects, and the GNSS signal quality is poor.

[0172] GSI is used to evaluate whether the foot movement of the current wearer is stable and reflects the credibility of the IMU data. The closer its value is to 1, the more stable the gait cycle and the more reliable the IMU prediction ability; the lower the value, the more irregular the movement or the more in a non-linear disturbance state.

[0173] To achieve dynamic adjustment of the GNSS observation data weight, the system constructs a fusion correction weight factor W, comprehensively considering the reliability of GNSS and IMU data in the current environment.

[0174] The construction formula of this factor is as follows: ; where: W is the observation noise correction coefficient; DSF and GSI are the direction scattering factor and the gait stability factor respectively; α and β are adjustment coefficients (the recommended value range of experience is α ∈ [1, 5], β ∈ [0.5, 3]), which are used to balance the influence of DSF and GSI on the overall fusion weight.

[0175] This formula reflects the following characteristics:

[0176] When DSF or GSI is low, it indicates that the GNSS data is not credible and W increases;

[0177] When both DSF and GSI are close to 1, it indicates that both the environment and the movement are good and W is close to 1.

[0178] In the extended Kalman filter, the GNSS observation data is input as a measurement item through the observation model, and its confidence is controlled by the observation noise covariance matrix R.

[0179] In the present invention, the following dynamic correction is performed on R: ; where:

[0180] R is the GNSS raw observation noise covariance matrix, usually a diagonal matrix, representing the variances of pseudorange and carrier phase observation errors; W is the dynamic correction factor defined in the previous section; R' is the corrected covariance matrix for actual filtering update.

[0181] Correction effect:

[0182] W increases → R' increases → Kalman gain decreases → the influence of GNSS observations weakens;

[0183] W decreases → R' decreases → the influence of GNSS observations strengthens.

[0184] This dynamic adjustment mechanism enables the filter to change the fusion strategy in real time according to the environmental quality and motion state, effectively reducing the positioning error and jump probability.

[0185] To further enhance the intelligent decision-making ability of the system, the present invention sets a joint confidence threshold T3 for guiding whether to switch the fusion strategy.

[0186] The joint judgment logic is as follows:

[0187] If DSF < T3 or GSI < T3 (e.g., T3 = 0.5), the system determines that the current GNSS or IMU confidence is insufficient; <00004​​​​​​​​​​​​​​​​​​​​​​​​​​​

[0196] Will and Used to calculate smoothing weights : This smoothing mechanism improves system stability and avoids drastic changes in weights due to sudden occlusion or gait jitter, thereby ensuring the continuity and reliability of the EKF filtering process.

[0197] Table 1. Classification of Positioning Results by Level

[0198]

[0199] This hierarchical mechanism can output information through graphical interfaces, voice broadcasts, system parameter transmissions, and other means, providing information for upper-level systems to make decisions and responses.

[0200] As shown in Table 1, when the location confidence score drops to the low confidence range (i.e., C_score < 0.5), the system will automatically take the following measures:

[0201] Stop or reduce the GNSS observation update frequency and temporarily freeze ambiguity solutions;

[0202] Increase the weight of the state transition equation in EKF, that is, strengthen the influence of IMU data in the prediction model;

[0203] Input thresholds are set for GNSS solution results, and smoothing buffering is implemented to prevent sudden jump interference.

[0204] This mode is suitable for scenarios where GNSS signals are blocked, terrain reflections are severe, or the user is stationary.

[0205] An alarm is triggered if the system maintains a C_score < 0.5 for a continuous period of T seconds (e.g., T = 3 seconds).

[0206] Alarm methods include: the control terminal sending a location anomaly flag; wearable devices emitting audible, visual, and vibration alerts; and the system log recording abnormal events for later analysis. This mechanism is suitable for ensuring personnel location safety in critical situations (such as underground operations, emergency rescue, and high-risk inspections).

[0207] Considering that GNSS or IMU data may experience short-term jitter, this invention also introduces a sliding scoring window mechanism to improve system stability, with the following steps:

[0208] Set the sliding window length N (N = 5 is recommended);

[0209] Record the latest N C_score values ​​in each epoch;

[0210] Calculate its weighted average Assign a credibility score to the current location;

[0211] It is used to replace single-point rating values ​​in the determination of grade classification and status switching.

[0212] This mechanism can effectively suppress misjudgments caused by "signal glitches" or "step transients".

[0213] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.

Claims

1. A precise positioning algorithm for intelligent iron shoes that integrates RTK and inertial navigation, characterized in that: include: Collect GNSS pseudorange observations, carrier phase observations, and IMU acceleration and angular velocity data, and perform timestamp alignment and preprocessing on them; Based on the GNSS pseudorange observations, the double difference observations of pseudorange and carrier phase are calculated using the real-time dynamic difference method. Combined with IMU data, preliminary RTK-INS fusion positioning is achieved through an extended Kalman filter to obtain preliminary position and attitude solutions. A directional distribution matrix is ​​constructed based on the unit vectors of each satellite direction in the acquired GNSS signal, its eigenvalues ​​are calculated, and the directional scattering factor (DSF) is determined based on the ratio of the maximum eigenvalue to the total eigenvalues. Based on the collected IMU acceleration data, the user's gait cycle characteristics are extracted. By comparing with historical gait templates, the cycle variance and correlation score of the current gait are calculated, and the gait stability factor GSI is constructed accordingly. The DSF and GSI are input as dynamically corrected weight parameters into the extended Kalman filter to adjust the observation noise covariance matrix, dynamically change the weight ratio in the RTK and inertial navigation data fusion process, and generate a confidence-weighted positioning result. Based on the output positioning results and their corresponding DSF and GSI indices, the positioning reliability score is calculated and divided into multiple levels. When the reliability is lower than the preset threshold, the system enters the inertial navigation priority mode or triggers an alarm mechanism to indicate abnormal positioning quality.

2. The intelligent track shoe precise positioning algorithm integrating RTK and inertial navigation according to claim 1, characterized in that: The process of aligning and preprocessing the data with timestamps includes: Iterate through the timestamps of each GNSS observation point; Find the nearest timestamp in the IMU data; If the time difference is less than the threshold Δt, then it is matched as the same time. If the difference exceeds the threshold Δt, then linear interpolation is performed to calculate the matching value of the IMU; Construct an observation matrix on a unified time axis for GNSS and IMU data.

3. The intelligent iron shoe precise positioning algorithm integrating RTK and inertial navigation according to claim 1, characterized in that: The calculation of the double-difference observations of pseudorange and carrier phase based on the GNSS pseudorange observations using a real-time dynamic difference method includes: Receive dual-frequency pseudorange and carrier phase observation data from GNSS base stations and mobile terminals; Based on the same time epoch, construct pseudorange single-difference and double-difference observation models between satellite and receiver; Common error terms, including satellite clock error, receiver clock error, and atmospheric delay error, are eliminated by constructing double-difference observations. The double-difference pseudorange and carrier phase observations are obtained for ambiguity fixing and position calculation.

4. The intelligent track shoe precise positioning algorithm integrating RTK and inertial navigation according to claim 3, characterized in that: The ambiguity is fixed, including: Construct an integer ambiguity resolution model for double-difference carrier phase observations; The least squares ambiguity solution algorithm is used to make a preliminary estimate of the integer ambiguity; Integer search of the estimated value based on fuzziness confidence interval; When the ambiguity solvability threshold condition is met, the ambiguity is fixed as an integer solution, and the preliminary positioning coordinates are output.

5. The intelligent iron shoe precise positioning algorithm integrating RTK and inertial navigation according to claim 1, characterized in that: The initial RTK-INS fusion localization is achieved by combining IMU data with an extended Kalman filter to obtain preliminary position and attitude solutions, including: RTK positioning results are used as system state observation inputs, and IMU acceleration and angular velocity are used as prediction inputs; Construct the system state transition equation and observation equation, and establish a state vector that includes position, velocity, attitude and sensor bias; The prediction and update process is executed iteratively, and the optimal state solution after fusion is estimated in real time. The output contains a fused solution including position coordinates, heading angle, and velocity vector.

6. The intelligent iron shoe precise positioning algorithm integrating RTK and inertial navigation according to claim 1, characterized in that: The calculation of the directional scattering factor DSF includes: Obtain the direction vectors of the n available GNSS satellites in the current epoch, normalize them, and form a set of direction vectors; All unit direction vectors are used to form a direction observation matrix V, with dimensions of n rows and 3 columns; Calculate the covariance matrix S of the direction observation matrix V, where S is equal to the product of the transpose of V and V. Perform eigenvalue decomposition on matrix S to obtain three real eigenvalues, denoted as , ... ; DSF equals 1 minus the ratio of the largest eigenvalue to the total eigenvalues.

7. The intelligent iron shoe precise positioning algorithm integrating RTK and inertial navigation according to claim 1, characterized in that: The calculation of the gait stability factor GSI includes: Based on the collected IMU triaxial acceleration data, the vertical acceleration signal sequence within the user's continuous walking cycle is extracted; The peak detection method is used to identify key step points within the gait cycle, including the foot strike point and the departure point; Calculate the time interval of several consecutive gait cycles to obtain periodic sequence data; The periodic variance is calculated from the periodic sequence; Standard gait templates are extracted from historical normal gait data to construct reference periodic waveforms; The time alignment distance between the current gait signal and the template is calculated using a dynamic time warping algorithm. The variance of the gait cycle and the correlation score of the dynamic time warping algorithm are used as inputs; The gait stability factor GSI is calculated by combining the negative periodic variance exponent of e with the correlation score of the dynamic time warping algorithm.

8. The intelligent iron shoe precise positioning algorithm integrating RTK and inertial navigation according to claim 1, characterized in that: The DSF and GSI are input as dynamically adjusted weight parameters into the extended Kalman filter to adjust the observation noise covariance matrix, including: In each fusion epoch, the directional scattering factor DSF and the gait stability factor GSI are calculated respectively. The DSF and GSI are both used as weight correction factors W and input to the observation noise covariance matrix adjustment module. The expression for calculating the weight correction factor W is: Where α and β are adjustable weighted sensitivity coefficients; In the update step of the extended Kalman filter, the original value of the covariance matrix R of the GNSS related observations is multiplied by the correction factor W to obtain the corrected observation noise covariance matrix.

Images