Gait detection and updating method based on inertial navigation system

Abstract

The invention relates to the field of inertial pedestrian navigation systems, and particularly discloses a gait detection and updating method based on an inertial navigation system, which comprises the following steps: S1, calculating calibration parameters of an accelerometer and a gyroscope by using an improved six-surface calibration method, and calculating parameters of a magnetometer by using an ellipsoid fitting method; s2, substituting parameters to update a measured value; s3, setting a sliding window, calculating an acceleration amplitude, a variance and an angular velocity amplitude, and comparing a threshold value to obtain an initial zero velocity parameter T; s4, continuous T is counted, and a final zero-speed interval is judged when a threshold value is exceeded; s5, inputting related parameters, updating positions by a quaternion method and the like, and calculating an attitude conversion matrix and an attitude angle; s6, correcting the course angle according to the amplitude of the magnetometer; s7, updating the error covariance and the Kalman gain matrix; and S8, constructing an error vector according to the zero-speed interval, inputting an extended Kalman filter to update parameters, and outputting a result. According to the gait detection and updating method based on the inertial navigation system, the problems of error accumulation and high cost of a traditional method are solved, and the navigation precision is effectively improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of inertial pedestrian navigation system, in particular to a gait detection and updating method based on inertial navigation system. BACKGROUND

[0002] Inertial navigation system (INS) has been widely applied in fire rescue, personnel tracking, medical care management and many other fields. Unlike global navigation satellite system (GNSS) which is susceptible to physical, electromagnetic signal interference and environmental factors, INS does not rely on external information, and can obtain acceleration and angular velocity data through accelerometers, gyroscopes and other sensors, and then calculate the velocity, position and attitude of the carrier, so as to realize target positioning in various scenarios.

[0003] Existing navigation technologies include satellite navigation, indoor navigation and inertial navigation. Among them, the signal anti-interference ability of satellite navigation is poor, and the signal coverage is not comprehensive, which is difficult to apply to accurate positioning in indoor and other complex scenarios; indoor positioning technologies including ultra-wideband, radio frequency identification, Bluetooth and the like also have the defects of short recognition distance, high cost and poor stability; on the other hand, although inertial navigation algorithm can be widely applied in many fields, it is still in the stage of development and research, and its recognition performance and stability still have room for improvement. How to calibrate sensor errors, detect zero speed interval and correct errors is a problem to be solved.

[0004] However, the existing inertial navigation method has the defects that the error accumulates with time and the recognition performance depends on high-precision measurement units, which reduces the positioning accuracy. Therefore, how to ensure the efficient, stable and reliable inertial navigation in various complex environments is a key and difficult point to be solved. SUMMARY

[0005] The present application provides a gait detection and updating method based on inertial navigation system to effectively solve the problems of low accuracy, high cost and limited application of traditional inertial navigation algorithm, effectively improve the navigation accuracy, and realize attitude calculation and position estimation.

[0006] To achieve the above purpose, the present application provides a gait detection and updating method based on inertial navigation system, comprising: S1, using an improved six-surface calibration method to calculate the calibration parameters of the accelerometer and the gyroscope, and using an ellipsoid fitting method to calculate the calibration parameters of the magnetometer; S2, substituting the calibration parameters obtained in S1 into the original measurement values of the accelerometer, the gyroscope and the magnetometer respectively to complete the measurement value updating; S3, set a sliding window, calculate the amplitude of the updated acceleration, the variance of the acceleration amplitude and the amplitude of the angular velocity in the window, and compare the three with the preset threshold value respectively to obtain an initial zero speed interval judgment parameter T; S4, count the output results of T in a plurality of S3s in succession, and when the number of consecutive T exceeds a preset threshold value, determine that it is a final zero speed interval; S5, input the current position, speed, quaternion, acceleration mean, angular velocity mean, sampling rate and gravity acceleration vector, update the position, speed, quaternion by the quaternion method, and calculate the attitude conversion matrix, navigation system acceleration and roll angle, pitch angle and heading angle; S6, judge whether to perform heading correction according to the measurement amplitude of the magnetometer, and if the heading angle error exceeds a preset range, adjust the heading angle; S7, update the error covariance matrix and the Kalman gain matrix based on the calculation results of S5; S8, according to the zero speed interval determination result of S4, construct an error state vector and input an extended Kalman filter, update the position, speed, attitude and angular velocity parameters, and output the final detection and update results.

[0007] Preferably, in S1, the improved six-surface calibration method specifically comprises: S111, record the acceleration data of each of the 6 surfaces of the positioning module for 2 minutes, take the median of the acceleration values obtained by each surface, and combine them, taking a single sensor as an example, the maximum acceleration value is a 1max , the minimum value is a 1min , the zero offset error coefficient b1 is calculated as: b1=(a 1min +a 1max ) / 2; the scale error factor k1=(a 1max -a 1min ) / 2; S112, according to the results of S11, calculate the corrected output value of the accelerometer: acc_s1n=k1*acc_s1-b1; In the formula, acc_s1 is the original measurement value of the accelerometer, and acc_s1n is the corrected measurement value; S113, collect the gyroscope data of the sensor in a single upward static state, calculate the average value as the initial zero offset gyr_bias1, and calculate the corrected output value of the gyroscope: gyr_s1n=gyr_s1-gyr_bias1; In the formula, gyr_s1 is the original measurement value of the gyroscope, and gyr_s1n is the corrected output value.

[0008] Preferably, in S1, the ellipsoid fitting method for calculating the calibration parameters of the magnetometer specifically comprises: S121, acquire the observation values of the x, y, z axes of the magnetometer rotating around a fixed point, and establish an ellipsoid fitting equation: ((x-x0) / A)²+((y-y0) / B)²+((z-z0) / C)²=1; In the formula, x0, y0, and z0 are the coordinates of the center of the ellipsoid, A is the equatorial radius along the x axis, B is the equatorial radius along the y axis, and C is the polar radius along the y axis; S122, expand the ellipsoid fitting equation into a general formula: x²+ay²+bz²+cx+dy+ez+f=0; In the formula, a, b, c, d, e, and f are unknowns to be solved in the least squares method, and a=(A / B)², b=(A / C)², c=-2x0, d=-2y0(A / B)², e=-2z0(A / C)², and f=x0²+(Ay0 / B)²+(Az0 / C)²-A²; S123, define an error function ei=xi²+ayi²+bzi²+cxi+dyi+ezi+f, xi, yi, and zi are the i-th magnetometer measurement values, express ei as the form of Ax-b, that is, x=[a,b,c,d,e,f]ᵀ, A=[yi²,zi²,xi,yi,zi,1], b=-xi², and solve x*=(AᵀA)⁻¹Aᵀb by the least squares method to obtain a, b, c, d, e, and f; S124, inversely deduce x0, y0, z0, A, B, and C according to a, b, c, d, e, and f, and calculate the corrected value of the magnetometer as: magxnew=(magx-x0) / A; magynew=(magy-y0) / B; magznew=(magz-z0) / C; In the formula, magx is the original observation value of the x axis of the magnetometer, magy is the original observation value of the y axis of the magnetometer, and magz is the original observation value of the z axis of the magnetometer.

[0009] Preferably, in S3, a sliding window is set, the amplitude of the updated acceleration in the window, the variance of the acceleration amplitude, and the amplitude of the angular velocity are calculated, and the three are compared with preset thresholds to obtain the specific steps of the initial zero-speed interval judgment parameter T. S31, set the size of the sliding window as w, and calculate the sliding average amplitude of the acceleration in the window at the corrected k time: ; In the formula, acc_s1nx is the corrected x-axis acceleration component, acc_s1ny is the corrected y-axis acceleration component, and acc_s1nz is the corrected z-axis acceleration component; S32, preset acceleration threshold values σ1 and σ2, calculate a binarization function based on the acceleration threshold value, for subsequent zero speed interval judgment F1(k) is: ; S33, calculate the corrected angular velocity amplitude gyr m in the window at time k: ; In the formula, gyr_1nx is the corrected x-axis angular velocity component, gyr_1ny is the corrected y-axis angular velocity component, gyr_1nz is the corrected z-axis angular velocity component, preset angular velocity threshold value σ3, calculate a binarization function based on the angular velocity threshold value, for subsequent zero speed interval judgment F2(k) is: ; S34, calculate the acceleration amplitude variance acc s : ; In the formula, acc w is the average acceleration in the window; Preset acceleration variance threshold value σ4, calculate a binarization function based on the acceleration variance threshold value, for subsequent zero speed interval judgment F3(k) is: ; S35, set the initial zero speed interval judgment parameter T=F1(k)&F2(k)&F3(k), T=1 indicates preliminary determination of zero speed, T=0 indicates non-zero speed.

[0010] Preferably, in S4, the output results of T in a plurality of S3 are counted, and when the number of continuous T exceeds a preset threshold value, it is determined that the final zero speed interval is specific: the preset threshold value is σ5, when the output results of a plurality of T are 1, it is determined that the current interval is the final zero speed interval, recorded as T out =1; otherwise, T out =0, it is determined as a non-zero speed interval.

[0011] Preferably, the calculation formula of the final zero speed interval determination result T out is: ; In the formula, T out =1 indicates a zero speed interval, and T out =0 indicates a non-zero speed interval.

[0012] Preferably, in S5, the process of updating the position, velocity, and quaternion through the quaternion method needs to convert the acceleration from the carrier coordinate system to the navigation coordinate system through the attitude conversion matrix, and then obtain the velocity and position update values through integral operation.

[0013] Preferably, in S5, a rotation matrix is calculated according to the roll and pitch angles, the magnetometer measurement in the navigation coordinate system is converted to the geographic coordinate system, and the measured heading angle is calculated as follows: ; In the formula, mag n is the corrected magnetometer measurement, C is the attitude matrix, Md is the local magnetic declination angle, magn_x and magn_y are the x-axis and y-axis components of magn, respectively.

[0014] Preferably, in S6, whether to perform heading correction is determined according to the measured amplitude of the magnetometer. If the error of the heading angle exceeds a preset range, the heading angle is adjusted as follows: S61, the measured amplitude mag m of the magnetometer in the window is calculated. ; ; If mag m is in the normal measurement range, the heading correction is performed. S62, let yaw_c be the heading angle calculated by the quaternion method. If the error of the heading angle exceeds the value range, the heading angle needs to be corrected, and the corrected heading angle is as follows: ; In the formula, is the difference between the calculated value and the measured value of the heading angle.

[0015] Preferably, in S8, according to the determination result of the zero-speed interval in S4, an error state vector is constructed and input into an extended Kalman filter, and the position, speed, attitude and angular velocity parameters are updated, and the final detection and update results are output, which specifically include: S81, if it is a zero-speed interval, i.e., T out = 1, the measured pitch and roll angles are calculated as follows: ; ; S82, the position, attitude, speed and angular velocity errors are constructed into an error state vector as follows: err_state = K * [zErr; ztErr; vErr; gyrErr]; In the formula, zErr is the difference between the calculated value and the measured value of the height, ztErr is the difference between the calculated value and the measured value of the roll angle, pitch angle and heading angle, vErr is the calculated speed value, gyrErr is the average angular velocity in the window, and K is the Kalman gain matrix. S83, pass the err_state into the Kalman filter to update parameters: position = position - position error component in err_state, velocity = velocity - velocity error component in err_state, angular velocity = angular velocity - angular velocity error component in err_state, correct the attitude matrix by the attitude error component in err_state; S84, if it is a non-zero speed interval, that is, Tout=0, set zErr=0, ztErr=0, vErr=0, gyrErr=0, and no error correction is performed.

[0016] Therefore, the application proposes a gait detection and updating method based on an inertial navigation system, which has the following beneficial effects: (1) Precise correction of sensor errors: The accelerometer, gyroscope and magnetometer errors are processed by improving the calibration method to provide high-precision data for gait detection and navigation, solving the problem of traditional positioning deviation.

[0017] (2) Reliable zero speed detection at low cost: Only relying on inertial sensor parameters to complete zero speed determination, without additional equipment, reducing hardware cost, while ensuring detection stability, suitable for low-cost scenarios.

[0018] (3) Reduce error accumulation: Combined with heading correction and extended Kalman filter, the parameters are corrected in the zero speed interval, effectively suppressing error accumulation over time, improving attitude solution accuracy and positioning reliability.

[0019] The technical solutions of the application will be further described in detail below with the help of the accompanying drawings and examples. BRIEF DESCRIPTION OF DRAWINGS

[0020] Figure 1 is a schematic diagram of an inertial navigation module and wearing method of a gait detection and updating method based on an inertial navigation system of the application; Figure 2 is an acceleration and gyroscope test data calibration simulation diagram of a gait detection and updating method based on an inertial navigation system of the application, wherein (a) is an acceleration test data calibration simulation diagram, and (b) is a gyroscope test data calibration simulation diagram; Figure 3 is a magnet calibration diagram of a gait detection and updating method based on an inertial navigation system of the application; Figure 4 is a zero speed detection result diagram of a gait detection and updating method based on an inertial navigation system of the application; Figure 5 is a heading angle output result diagram of a gait detection and updating method based on an inertial navigation system of the application; Figure 6It is a gait detection and updating algorithm of a gait detection and updating method based on an inertial navigation system; Figure 7 It is a test result graph of the gait detection and updating method based on the inertial navigation system. DETAILED DESCRIPTION

[0021] In order to make the technical solutions, advantages and purposes of the present application clearer, the technical solutions of the embodiments of the present application will be described clearly and completely below. The described embodiments are part of the embodiments of the present application, rather than all the embodiments. Based on the described embodiments of the present application, all other embodiments obtained by those of ordinary skill in the art without any creative effort belong to the protection scope of the present application.

[0022] Unless otherwise defined, the technical terms or scientific terms used in the present application shall have the usual meanings understood by those of ordinary skill in the art to which the present application belongs.

[0023] As shown in the accompanying drawings, Figures 1-7 The present application provides a gait detection and updating method based on an inertial navigation system, which comprises: S1, using an improved six-surface calibration method to calculate the calibration parameters of the accelerometer and the gyroscope, and using an ellipsoid fitting method to calculate the calibration parameters of the magnetometer; S2, substituting the calibration parameters obtained in S1 into the original measurement values of the accelerometer, the gyroscope and the magnetometer respectively to complete the measurement value updating; S3, setting a sliding window, calculating the amplitude of the updated acceleration, the acceleration amplitude variance and the angular velocity amplitude in the window, and comparing the three with the preset threshold value respectively to obtain an initial zero speed interval judgment parameter T; S4, counting the output results of T in a plurality of S3s in succession, and determining the final zero speed interval when the number of consecutive T exceeds the preset threshold value; S5, inputting the current position, speed, quaternion, acceleration mean value, angular velocity mean value, sampling rate and gravity acceleration vector, updating the position, speed and quaternion by the quaternion method, and calculating the attitude conversion matrix, the navigation system acceleration and the roll angle, the pitch angle and the heading angle; S6, judging whether to perform heading correction according to the measurement amplitude of the magnetometer, and adjusting the heading angle if the heading angle error exceeds the preset range; S7, updating the error covariance matrix and the Kalman gain matrix based on the calculation results of S5; S8, constructing an error state vector according to the zero speed interval determination result of S4 and inputting the extended Kalman filter to update the position, speed, attitude and angular velocity parameters, and outputting the final detection and updating result.

[0024] In S1, the improved six-surface calibration method specifically comprises: S111, record the acceleration data of each surface of the positioning module 6 for 2 minutes, take the median of the acceleration values obtained by each surface, and take the maximum acceleration value as an example, a 1max , the minimum value is a 1min , calculate the zero offset error coefficient b1 as: b1=(a 1min +a 1max ) / 2; the scale error factor k1=(a 1max -a 1min ) / 2; S112, according to the result of S11, calculate the corrected output value of the accelerometer: acc_s1n=k1*acc_s1-b1; In the formula, acc_s1 is the original measurement value of the accelerometer, and acc_s1n is the corrected measurement value; S113, collect the gyroscope data of the sensor facing upward and stationary in a single surface, calculate the average value of the data as the initial zero offset gyr_bias1, and calculate the corrected output value of the gyroscope: gyr_s1n=gyr_s1-gyr_bias1; In the formula, gyr_s1 is the original measurement value of the gyroscope, and gyr_s1n is the corrected output value.

[0025] In S1, the ellipsoid fitting method for calculating the calibration parameters of the magnetometer specifically comprises: S121, obtain the x, y, z axis observation values of the magnetometer rotating around a fixed point, and establish an ellipsoid fitting equation: ((x-x0) / A)²+((y-y0) / B)²+((z-z0) / C)²=1; In the formula, x0, y0, z0 are the coordinates of the ellipsoid center, A is the equatorial radius along the x axis, B is the equatorial radius along the y axis, and C is the polar radius along the y axis; S122, expand the ellipsoid fitting equation into a general formula: x²+ay²+bz²+cx+dy+ez+f=0; In the formula, a, b, c, d, e, f are unknown numbers to be solved in the least squares method, and a=(A / B)², b=(A / C)², c=-2x0, d=-2y0(A / B)², e=-2z0(A / C)², f=x0²+(Ay0 / B)²+(Az0 / C)²-A²; S123. Define the error function ei=xi²+ayi²+bzi²+cxi+dyi+ezi+f, where xi, yi, and zi are the values ​​of the i-th magnetometer measurement. Express ei in the form Ax-b, i.e., x=[a,b,c,d,e,f]ᵀ, A=[yi²,zi²,xi,yi,zi,1], b=-xi². Solve for x*=(AᵀA)⁻¹Aᵀb using the least squares method to obtain a, b, c, d, e, and f. S124. Based on a, b, c, d, e, and f, inversely deduce x0, y0, z0, A, B, and C, and calculate the calibrated value of the magnetometer: magxnew=(magx-x0) / A; magynew=(magy-y0) / B; magznew=(magz-z0) / C; In the formula, magx is the original x-axis observation value of the magnetometer, magy is the original y-axis observation value of the magnetometer, and maz is the original z-axis observation value of the magnetometer.

[0026] In S3, the specific steps for setting a sliding window, calculating the magnitude of acceleration, the variance of acceleration magnitude, and the magnitude of angular velocity after the update within the window, and comparing these three values ​​with preset thresholds to obtain the initial zero-velocity interval judgment parameter T include: S31. Set the sliding window size to w, and calculate the sliding average magnitude of acceleration within the window at time k after correction: ; In the formula, acc_s1nx is the x-axis acceleration component after correction, acc_s1ny is the y-axis acceleration component after correction, and acc_s1nz is the z-axis acceleration component after correction. S32. Preset acceleration thresholds σ1 and σ2, calculate the binarization function based on the acceleration thresholds, and use it for subsequent zero-velocity interval judgment F1(k): ; S33. Calculate the amplitude of the angular velocity gyr within the window after correction at time k. m : ; In the formula, gyr_1nx is the corrected x-axis angular velocity component, gyr_1ny is the corrected y-axis angular velocity component, and gyr_1nz is the corrected z-axis angular velocity component. A preset angular velocity threshold σ3 is used to calculate a binarization function based on the angular velocity threshold, which is then used for subsequent zero-velocity interval determination. F2(k) is: ; S34. Calculate the variance of acceleration amplitude (acc) s : ; In the formula, acc w The average acceleration within the window; A preset acceleration variance threshold σ4 is used to calculate a binarization function based on the acceleration variance threshold, which is then used for subsequent zero-velocity interval determination. F3(k) is: ; S35. Set the initial zero-speed interval judgment parameter T=F1(k)&F2(k)&F3(k). When T=1, it indicates that the initial judgment is zero speed, and when T=0, it indicates that the speed is not zero.

[0027] In S4, the output results of T in multiple consecutive S3s are counted. When the number of consecutive Ts exceeds a preset threshold, it is determined to be the final zero-speed interval. Specifically, the preset threshold is σ5. When the output results of multiple consecutive Ts are 1, the current interval is determined to be the final zero-speed interval, denoted as T. out =1; otherwise T out =0, indicating a non-zero speed range.

[0028] Final zero-speed range determination result T out The calculation formula is: ; In the formula, T out =1 indicates the zero-speed interval, T out =0 indicates a non-zero speed range.

[0029] In S5, the process of updating position, velocity, and quaternions using the quaternion method requires combining the attitude transformation matrix to convert the acceleration from the carrier coordinate system to the navigation coordinate system, and then obtaining the velocity and position update values ​​through integration.

[0030] In S5, a rotation matrix is ​​calculated based on roll and pitch angles to transform the magnetometer measurements in the navigation coordinate system to the geographic coordinate system, and the measured heading angle is calculated accordingly. ; In the formula, mag n The values ​​are the calibrated magnetometer measurements, C is the attitude matrix, Md is the local magnetic declination, and magn_x and magn_y are the x and y axis components of magn, respectively.

[0031] In S6, the determination of whether to perform heading correction is based on the measurement amplitude of the magnetometer. If the heading angle error exceeds the preset range, the heading angle is adjusted as follows: S61. Calculate the magnetometer measurement amplitude (mag) within the window. m : ; ; If magm If the measurement range is within the normal range, then a course correction will be made; S62. Let yaw_c be the heading angle calculated by the quaternion method. If the error of the heading angle exceeds the range, it needs to be corrected. The correction is as follows: ; In the formula, This is the difference between the calculated and measured heading angle.

[0032] In S8, based on the zero-velocity interval determination result of S4, an error state vector is constructed and input into the extended Kalman filter to update the position, velocity, attitude, and angular velocity parameters. The final detection and update results are output, specifically including: S81, If ​​it is the zero speed range, i.e., T out =1, calculate and measure the pitch and roll angles as follows: ; ; S82. Construct an error state vector from the position, attitude, velocity, and angular velocity errors: err_state=K*[zErr;ztErr;vErr;gyrErr]; In the formula, zErr is the difference between the calculated and measured altitude, ztErr is the difference between the calculated and measured roll angle, pitch angle, and yaw angle, vErr is the calculated velocity value, gyrErr is the mean angular velocity within the window, and K is the Kalman gain matrix. S83. Pass err_state into the Kalman filter to update the parameters: position = position - position error component in err_state, velocity = velocity - velocity error component in err_state, angular velocity = angular velocity - angular velocity error component in err_state. Correct the attitude matrix through the attitude error component in err_state. S84. If the speed range is non-zero, i.e., Tout=0, set zErr=0, ztErr=0, vErr=0, gyrErr=0, and do not perform error correction.

[0033] Example 1 like Figure 1 As shown, this invention utilizes the designed positioning module to acquire actual data by wearing it on the foot, thereby completing simulation tests and performance verification of each part of the algorithm.

[0034] like Figure 6 As shown, the specific implementation steps of the zero-rate detection and update method include: S1. The calibration parameters of the accelerometer and gyroscope are calculated using the improved six-sided calibration method, and the calibration parameters of the magnetometer are calculated using the ellipsoid fitting method. Simulation of acceleration and gyroscope calibration, such as Figure 2 As shown. Data was recorded from six faces of the positioning module remaining stationary for two minutes. The median acceleration values ​​from each face were then combined to calculate the correction parameters. Taking sensor 1 as an example, let a... 1min and a 1max Let b1 represent the maximum and minimum values ​​of acceleration, respectively. The error coefficient b1 for zero bias is calculated as follows: b1 = (a 1min +a 1max ) / 2; Meanwhile, the scaling error factor k1 is calculated as follows: k1=(a 1max -a 1min ) / 2; The accelerometer output value acc_s1n after calibration is calculated as follows: acc_s1n = k1 * acc_s1 - b1; For gyroscope error correction, single-plane stationary data with the sensor facing upwards is selected. The average value is used as the initial zero bias gyr_bias1. The final calibration result gyr_s1n is the original gyr_s1 value measured by the gyroscope minus the initial zero bias. The calculation formula is as follows: gyr_s1n = gyr_s1 - gyr_bias1.

[0035] The simulation results after magnetometer calibration are as follows Figure 3 As shown. The ellipsoid fitting method for calculating magnetometer calibration parameters specifically includes: Obtain the x, y, and z axis observations of the magnetometer rotating around a fixed point, and establish the ellipsoid fitting equation: ((x-x0) / A)²+((y-y0) / B)²+((z-z0) / C)²=1; In the formula, x0, y0, z0 are the coordinates of the center of the ellipsoid, A is the equatorial radius along the x-axis, B is the equatorial radius along the y-axis, and C is the polar radius along the y-axis. Expanding the ellipsoid fitting equation into the general form: x² + ay² + bz² + cx + dy + ez + f = 0; In the formula, a, b, c, d, e, and f are the unknowns to be solved in the least squares method, and a = (A / B)², b = (A / C)², c = -2x0, d = -2y0(A / B)², e = -2z0(A / C)², f = x0² + (Ay0 / B)² + (Az0 / C)² - A²; Define the error function ei = xi² + ayi² + bzi² + cxi + dyi + ezi + f, where xi, yi, and zi are the values ​​of the i-th magnetometer measurement. Express ei in the form Ax - b, i.e., x = [a, b, c, d, e, f]ᵀ, A = [yi², zi², xi, yi, zi, 1], and b = -xi². Solve for x* = (AᵀA)⁻¹Aᵀb using the least squares method to obtain a, b, c, d, e, and f. Based on a, b, c, d, e, and f, we can deduce x0, y0, z0, A, B, and C, and calculate the calibrated values ​​of the magnetometer: magxnew=(magx-x0) / A; magynew=(magy-y0) / B; magznew=(magz-z0) / C; In the formula, magx is the original x-axis observation value of the magnetometer, magy is the original y-axis observation value of the magnetometer, and maz is the original z-axis observation value of the magnetometer.

[0036] S2. Substitute the calibration parameters obtained in S1 into the original measurement values ​​of the accelerometer, gyroscope, and magnetometer respectively to complete the measurement value update; In this embodiment, the zero-velocity detection of the standing phase is performed using the signal characteristics of an inertial sensor during pedestrian movement, and then error correction is performed within the zero-velocity range. Specifically, various parameters such as acceleration amplitude, acceleration amplitude variance, and angular velocity amplitude within a sliding window are calculated, and threshold values ​​for each parameter are obtained through multiple experiments. By comparing with the threshold values, the zero-velocity range during pedestrian movement is determined.

[0037] S3. Set a sliding window, calculate the magnitude of acceleration, variance of acceleration magnitude, and magnitude of angular velocity within the window after the update, and compare the three with preset thresholds to obtain the initial zero-velocity interval judgment parameter T. The specific steps are as follows: S31. Set the sliding window size to w, and calculate the sliding average magnitude of acceleration within the window at time k after correction: ; In the formula, acc_s1nx is the x-axis acceleration component after correction, acc_s1ny is the y-axis acceleration component after correction, and acc_s1nz is the z-axis acceleration component after correction. S32. Preset acceleration thresholds σ1 and σ2, calculate the binarization function based on the acceleration thresholds, and use F1(k) for subsequent zero-velocity interval determination: ; S33. Calculate the amplitude of the angular velocity gyr within the window after correction at time k. m : ; In the formula, gyr_1nx is the corrected x-axis angular velocity component, gyr_1ny is the corrected y-axis angular velocity component, and gyr_1nz is the corrected z-axis angular velocity component. A preset angular velocity threshold σ3 is used to calculate a binarized function based on the angular velocity threshold, which is then used for subsequent zero-velocity interval determination. F2(k) is: ; S34. Calculate the variance of acceleration amplitude (acc) s : ; In the formula, acc w The average acceleration within the window; A preset acceleration variance threshold σ4 is used to calculate a binarization function based on the acceleration variance threshold, which is then used for subsequent zero-velocity interval determination. F3(k) is: ; S35. Set the initial zero speed interval judgment parameter T=F1(k)&F2(k)&F3(k)&F4(k). When T=1, it indicates that the initial judgment is zero speed, and when T=0, it indicates that the speed is not zero.

[0038] S4. Count the output results of T in multiple consecutive S3s. When the number of consecutive Ts exceeds a preset threshold, it is determined to be the final zero-speed interval. Specifically: the preset threshold is σ5. When the output results of multiple consecutive Ts are 1, the current interval is determined to be the final zero-speed interval, denoted as T. out =1; otherwise T out =0, indicating a non-zero speed range.

[0039] Among them, the final zero-speed interval determination result T out The calculation formula is: ; In the formula, T out =1 indicates the zero-speed interval, T out =0 indicates a non-zero speed range.

[0040] In this embodiment, through extensive testing in walking and running postures, σ1 to σ5 were set to 8, 14, 1.5, 1.34 and 2 respectively. Figure 4 The above algorithm detects zero velocity in motion, with green dots representing the zero velocity range.

[0041] S5. Input the current position, velocity, quaternion, mean acceleration, mean angular velocity, sampling rate, and gravitational acceleration vector. Update the position, velocity, and quaternion using the quaternion method, and calculate the attitude transformation matrix, navigation system acceleration, roll angle, pitch angle, and yaw angle. The process of updating position, velocity, and quaternions using the quaternion method requires combining the attitude transformation matrix to convert the acceleration from the carrier coordinate system to the navigation coordinate system, and then obtaining the velocity and position update values ​​through integration.

[0042] The rotation matrix is ​​calculated based on the roll and pitch angles. The magnetometer measurements in the navigation coordinate system are then converted to the geographic coordinate system, and the measured heading angle is calculated accordingly. ; In the formula, mag n The values ​​are the calibrated magnetometer measurements, C is the attitude matrix, Md is the local magnetic declination, and magn_x and magn_y are the x and y axis components of magn, respectively.

[0043] S6. Determine whether to perform heading correction based on the magnetometer's measurement amplitude. If the heading angle error exceeds the preset range, adjust the heading angle as follows: S61. Calculate the magnetometer measurement amplitude (mag) within the window. m : ; ; If mag m If the measurement range is within the normal range, then a course correction will be made; S62. Let yaw_c be the heading angle calculated by the quaternion method. If the error of the heading angle exceeds the range, it needs to be corrected. The correction is as follows: ; In the formula, This is the difference between the calculated and measured heading angle.

[0044] Figure 5 The heading angle outputs are calculated using the quaternion method and the magnetometer, respectively, after heading correction. It can be seen that the difference between the two is quite close.

[0045] S7. Based on the calculation results of S5, update the error covariance matrix and Kalman gain matrix; S8. Based on the zero-velocity interval determination result of S4, construct the error state vector and input it into the extended Kalman filter to update the position, velocity, attitude, and angular velocity parameters, and output the final detection and update results, specifically including: S81, If ​​it is the zero speed range, i.e., T out =1, calculate and measure the pitch and roll angles as follows: ; ; S82. Construct an error state vector from the position, attitude, velocity, and angular velocity errors: err_state=K*[zErr;ztErr;vErr;gyrErr]; In the formula, zErr is the difference between the calculated and measured altitude, ztErr is the difference between the calculated and measured roll angle, pitch angle, and yaw angle, vErr is the calculated velocity value, gyrErr is the mean angular velocity within the window, and K is the Kalman gain matrix. S83. Pass err_state into the Kalman filter to update the parameters: position = position - position error component in err_state, velocity = velocity - velocity error component in err_state, angular velocity = angular velocity - angular velocity error component in err_state. Correct the attitude matrix through the attitude error component in err_state. S84. If the speed range is non-zero, i.e., Tout=0, set zErr=0, ztErr=0, vErr=0, gyrErr=0, and do not perform error correction.

[0046] Based on the above process, through Figure 1 An actual test was conducted around a table using the wearing method shown, and the trajectories of sensors 1, 2, 3, and the differential sensor were obtained as follows: Figure 7 As shown.

[0047] Therefore, this invention provides a gait detection and update method based on an inertial navigation system. By improving the six-sided calibration method to calibrate the accelerometer and gyroscope, and the ellipsoidal fitting method to calibrate the magnetometer, sensor errors are effectively compensated, providing high-precision data for subsequent calculations. Zero-velocity detection relies solely on inertial sensor parameters, requiring no additional hardware, reducing costs and ensuring stability. Combined with heading correction and extended Kalman filtering, position, velocity, and other parameters are corrected in the zero-velocity range, suppressing error accumulation and significantly improving attitude calculation and positioning accuracy, which can meet the pedestrian navigation needs of various scenarios.

[0048] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A gait detection and update method based on an inertial navigation system, characterized in that, include: S1. The calibration parameters of the accelerometer and gyroscope are calculated using the improved six-sided calibration method, and the calibration parameters of the magnetometer are calculated using the ellipsoid fitting method. S2. Substitute the calibration parameters obtained in S1 into the original measurement values ​​of the accelerometer, gyroscope, and magnetometer respectively to complete the measurement value update; S3. Set a sliding window, calculate the magnitude of acceleration, the variance of acceleration magnitude, and the magnitude of angular velocity after the update within the window, and compare the three with the preset thresholds to obtain the initial zero velocity interval judgment parameter T. S4. Statistically analyze the output results of T in multiple consecutive S3s. When the number of consecutive Ts exceeds a preset threshold, it is determined to be the final zero-speed interval. S5. Input the current position, velocity, quaternion, mean acceleration, mean angular velocity, sampling rate, and gravitational acceleration vector. Update the position, velocity, and quaternion using the quaternion method, and calculate the attitude transformation matrix, navigation system acceleration, roll angle, pitch angle, and yaw angle. S6. Determine whether to perform heading correction based on the measurement amplitude of the magnetometer. If the heading angle error exceeds the preset range, adjust the heading angle. S7. Based on the calculation results of S5, update the error covariance matrix and Kalman gain matrix; S8. Based on the zero-velocity interval determination result of S4, construct the error state vector and input it into the extended Kalman filter to update the position, velocity, attitude and angular velocity parameters, and output the final detection and update result.

2. The gait detection and update method based on an inertial navigation system according to claim 1, characterized in that, In S1, the improved six-sided calibration method specifically includes: S111. Record the acceleration data of each of the six faces of the positioning module after 2 minutes of stillness. Take the median of the acceleration values ​​obtained from each face and combine them. Taking a single sensor as an example, the maximum acceleration value is a. 1max The minimum value is a 1min The zero bias error coefficient b1 is calculated as follows: b1 = (a 1min +a 1max ) / 2; Scale error factor k1=(a 1max -a 1min ) / 2; S112. Based on the results of S11, calculate the accelerometer output value after calibration: acc_s1n = k1 * acc_s1 - b1; In the formula, acc_s1 is the original measurement value of the accelerometer, and acc_s1n is the measurement value after calibration; S113. Collect gyroscope data from a stationary gyroscope with the sensor facing upwards, calculate the average data value as the initial zero bias gyr_bias1, and calculate the gyroscope output value after calibration: gyr_s1n = gyr_s1 - gyr_bias1; In the formula, gyr_s1 is the original measurement value of the gyroscope, and gyr_s1n is the output value after correction.

3. The gait detection and update method based on an inertial navigation system according to claim 1, characterized in that, In S1, the calculation of the magnetometer calibration parameters using the ellipsoid fitting method specifically includes: S121. Obtain the x, y, and z axis observations of the magnetometer rotating around a fixed point, and establish the ellipsoid fitting equation: ((x-x0) / A)²+((y-y0) / B)²+((z-z0) / C)²=1; In the formula, x0, y0, z0 are the coordinates of the center of the ellipsoid, A is the equatorial radius along the x-axis, B is the equatorial radius along the y-axis, and C is the polar radius along the y-axis. S122. Expand the ellipsoid fitting equation into the general form: x² + ay² + bz² + cx + dy + ez + f = 0; In the formula, a, b, c, d, e, and f are the unknowns to be solved in the least squares method, and a = (A / B)², b = (A / C)², c = -2x0, d = -2y0(A / B)², e = -2z0(A / C)², f = x0² + (Ay0 / B)² + (Az0 / C)² - A²; S123. Define the error function ei=xi²+ayi²+bzi²+cxi+dyi+ezi+f, where xi, yi, and zi are the values ​​of the i-th magnetometer measurement. Express ei in the form Ax-b, i.e., x=[a,b,c,d,e,f]ᵀ, A=[yi²,zi²,xi,yi,zi,1], b=-xi². Solve for x*=(AᵀA)⁻¹Aᵀb using the least squares method to obtain a, b, c, d, e, and f. S124. Based on a, b, c, d, e, and f, inversely deduce x0, y0, z0, A, B, and C, and calculate the calibrated value of the magnetometer: magxnew=(magx-x0) / A; magynew=(magy-y0) / B; magznew=(magz-z0) / C; In the formula, magx is the original x-axis observation value of the magnetometer, magy is the original y-axis observation value of the magnetometer, and mazh is the original z-axis observation value of the magnetometer.

4. The gait detection and update method based on an inertial navigation system according to claim 1, characterized in that, In S3, the specific steps for setting a sliding window, calculating the magnitude of acceleration, the variance of acceleration magnitude, and the magnitude of angular velocity after the update within the window, and comparing these three values ​​with preset thresholds to obtain the initial zero-velocity interval judgment parameter T include: S31. Set the sliding window size to w, and calculate the sliding average magnitude of acceleration within the window at time k after correction: ； In the formula, acc_s1nx is the corrected x-axis acceleration component, acc_s1ny is the corrected y-axis acceleration component, and acc_s1nz is the corrected z-axis acceleration component. S32. Preset acceleration thresholds σ1 and σ2, calculate the binarization function based on the acceleration thresholds, and use it for subsequent zero-velocity interval judgment F1(k): ； S33. Calculate the amplitude of the angular velocity gyr within the window at time k after correction. m : ； In the formula, gyr_1nx is the corrected x-axis angular velocity component, gyr_1ny is the corrected y-axis angular velocity component, and gyr_1nz is the corrected z-axis angular velocity component. A preset angular velocity threshold σ3 is used to calculate a binarization function based on the angular velocity threshold, which is then used for subsequent zero-velocity interval determination. F2(k) is: ； S34. Calculate the variance of acceleration amplitude (acc) s : ； In the formula, acc w The average acceleration within the window; A preset acceleration variance threshold σ4 is used to calculate a binarization function based on the acceleration variance threshold, which is then used for subsequent zero-velocity interval determination. F3(k) is: ; S35. Set the initial zero-speed interval judgment parameter T=F1(k)&F2(k)&F3(k). When T=1, it indicates that the initial judgment is zero speed, and when T=0, it indicates that the speed is not zero.

5. The gait detection and update method based on an inertial navigation system according to claim 1, characterized in that, In S4, the output results of T in multiple consecutive S3s are counted. When the number of consecutive Ts exceeds a preset threshold, it is determined to be the final zero-speed interval. Specifically, the preset threshold is σ5. When the output results of multiple consecutive Ts are 1, the current interval is determined to be the final zero-speed interval, denoted as T. out =1; otherwise T out =0, indicating a non-zero speed range.

6. The gait detection and update method based on an inertial navigation system according to claim 5, characterized in that, Final zero-speed range determination result T out The calculation formula is: ； In the formula, T out =1 indicates the zero-speed interval, T out =0 indicates a non-zero speed range.

7. The gait detection and update method based on an inertial navigation system according to claim 1, characterized in that, In S5, the process of updating position, velocity, and quaternions using the quaternion method requires combining the attitude transformation matrix to convert the acceleration from the carrier coordinate system to the navigation coordinate system, and then obtaining the velocity and position update values ​​through integration.

8. The gait detection and update method based on an inertial navigation system according to claim 1, characterized in that, In S5, a rotation matrix is ​​calculated based on roll and pitch angles to transform the magnetometer measurements in the navigation coordinate system to the geographic coordinate system, and the measured heading angle is calculated accordingly. ； In the formula, mag n The values ​​are the calibrated magnetometer measurements, C is the attitude matrix, Md is the local magnetic declination, and magn_x and magn_y are the x and y axis components of magn, respectively.

9. The gait detection and update method based on an inertial navigation system according to claim 1, characterized in that, In S6, the determination of whether to perform heading correction is based on the measured amplitude of the magnetometer. If the heading angle error exceeds the preset range, the heading angle is adjusted as follows: S61. Calculate the magnetometer measurement amplitude (mag) within the window. m : ； ； If mag m If the measurement range is within the normal range, then a course correction will be made; S62. Let yaw_c be the heading angle calculated by the quaternion method. If the error of the heading angle exceeds the range, it needs to be corrected. The correction is as follows: ； In the formula, This is the difference between the calculated and measured heading angle.

10. The gait detection and update method based on an inertial navigation system according to claim 1, characterized in that, In S8, based on the zero-velocity interval determination result of S4, an error state vector is constructed and input into the extended Kalman filter to update the position, velocity, attitude, and angular velocity parameters. The final detection and update results are output, specifically including: S81, If ​​it is the zero speed range, i.e., T out =1, calculate and measure the pitch and roll angles as follows: ； ； S82. Construct an error state vector from the position, attitude, velocity, and angular velocity errors: err_state=K*[zErr;ztErr;vErr;gyrErr]; In the formula, zErr is the difference between the calculated and measured altitude, ztErr is the difference between the calculated and measured roll angle, pitch angle, and yaw angle, vErr is the calculated velocity value, gyrErr is the mean angular velocity within the window, and K is the Kalman gain matrix. S83. Pass err_state into the Kalman filter to update the parameters: position = position - position error component in err_state, velocity = velocity - velocity error component in err_state, angular velocity = angular velocity - angular velocity error component in err_state. Correct the attitude matrix through the attitude error component in err_state. S84. If the speed range is non-zero, i.e., Tout=0, set zErr=0, ztErr=0, vErr=0, gyrErr=0, and do not perform error correction.

Images