DTW-UKF collaborative attitude estimation method for multi-source noise

Abstract

The invention discloses a DTW-UKF collaborative attitude estimation method for multi-source noise, and the method comprises the steps: 1, carrying out the preprocessing of IMU data, and constructing a static time sequence and a dynamic time sequence based on the preprocessed data; 2, calculating an alignment path of the dynamic time sequence and the static time sequence through a multi-dimensional DTW algorithm, and calculating a time local difference and an inter-axis difference degree; 3, adaptively adjusting the noise covariance of the UKF; 4, defining a state variable of the UKF, generating a Sigma point set, and predicting the state variable and a covariance thereof; 5, updating posteriori estimation and covariance of the UKF by combining observation data; and repeating the steps 2-5 to obtain attitude estimation at all moments. According to the method, the analysis capability of the DTW algorithm on the multi-dimensional time sequence is utilized, the influence of dynamic disturbance is overcome, the attitude estimation precision is improved, and a high-precision and high-robustness method is provided for motion attitude estimation.

Description

Technical Field

[0001] The present invention belongs to the technical field of human posture motion analysis, and in particular relates to a DTW-UKF collaborative posture estimation method for multi-source noise. Background Art

[0002] In the field of sports analysis, real-time acquisition of athletes' motion posture is crucial for motion analysis and injury prevention. Attitude estimation methods based on inertial measurement units (IMUs) have emerged as a viable solution due to their real-time performance and occlusion resistance. However, strenuous exercise can produce significant acceleration, which can accumulate errors in pitch and roll angle estimation. Furthermore, magnetic field disturbances caused by metal objects or electronic devices in the playing field can cause the magnetometer output to deviate from the true geomagnetic vector, increasing the error in the heading angle calculation. Therefore, filtering-based methods have been proposed to fuse multi-sensor data to improve attitude estimation accuracy. Among them, the unscented Kalman filter (UKF) effectively suppresses Gaussian noise by approximating the statistical characteristics of nonlinear systems through sigma point sampling. However, the UKF relies on an empirically set noise covariance matrix and cannot effectively overcome the effects of dynamic noise.

[0003] In order to enhance the adaptive ability of the filter, the study proposed an adaptive filtering method based on thresholds or models, which dynamically adjusts the noise covariance matrix by analyzing the statistical characteristics of the noise. However, this type of method only focuses on the noise characteristics on the time scale and does not fully consider the inter-axis difference characteristics of the multi-dimensional data within the sensor. Specifically, different axes within a single sensor may exhibit heterogeneous noise distribution due to external disturbances, making it difficult for traditional adaptive strategies to accurately characterize the noise.

[0004] In order to solve the above problems, the present invention proposes a DTW-UKF collaborative attitude estimation method for multi-source noise. Summary of the Invention

[0005] In order to overcome the shortcomings of the existing technology, the purpose of the present invention is to provide a DTW-UKF collaborative attitude estimation method for multi-source noise. The dynamic time warping (DTW) algorithm is used to align multidimensional time series and analyze the data of a single sensor. By calculating the noise fluctuations within a local time window (local temporal differences) and the noise distribution differences between different axes (inter-axis differences), the covariance of the filter is adaptively adjusted to match the intensity of the external disturbance, thereby achieving robust attitude estimation.

[0006] The technical problem solved by the present invention can be achieved through the following specific technical solutions:

[0007] The DTW-UKF collaborative attitude estimation method for multi-source noise includes the following steps:

[0008] Step 1: Preprocess the IMU data and construct static time series and dynamic time series based on the preprocessed data;

[0009] Step 2: Calculate the alignment path of the dynamic time series D and the static time series T using the multidimensional DTW algorithm, and calculate the local temporal difference and inter-axis difference.

[0010] Step 3: Adaptively adjust the noise covariance of the UKF according to the calculated temporal local difference and inter-axis difference;

[0011] Step 4: Define the state variables of UKF, generate Sigma point set, and predict the state variables and its covariance

[0012] Step 5: Update the posterior estimate of UKF and its covariance by combining the observed data;

[0013] Repeat steps 2-5 to obtain the pose estimation at all times.

[0014] Furthermore, in step 1, the preprocessing process normalizes the mean and standard deviation of the dynamic data based on the IMU static reference information, and the process is given by the following formula:

[0015]

[0016] Where y norm is the normalized dynamic data, namely angular velocity, acceleration and magnetic field vector; y is the original dynamic data, μ static , σ static are the mean and standard deviation of the static benchmark information respectively; a static time series is established based on the mean, which consists of zero angular velocity, gravitational acceleration and geomagnetic field vector.

[0017] Furthermore, in step 1, a time sliding window is used to segment static data and dynamic data in real time, and a static time series and a dynamic time series are obtained, which are represented by T = {t1, t2, t3, ..., t n}, D={d1,d2,d3,...,d m}, the length of the time sliding window is 6 time steps, and the overlap rate of the window is 50%.

[0018] Furthermore, in step 2, the specific calculation process is as follows:

[0019] ① Define the multidimensional weighted Euclidean distance dist(·) of the DTW algorithm:

[0020]

[0021] Where, ω κ is the weight coefficient of each axis, d i , t j Represent the dynamic time series and static time series of each time window respectively, and Represents the dynamic time series and static time series of each axis of the IMU;

[0022] ② Calculate the alignment path of the dynamic time series D and the static time series T using the DTW algorithm. The process is given by the following formula:

[0023] Γ i,j =dist(d i ,t j )+min(Γ i-1,j ,Γ i,j-1 ,Γ i-1,j-1 ) (3)

[0024] ρ={(p1,q1),(p2,q2),(p3,q3),...,(p n ,q n )} (4)

[0025] Where, Γ i,j , Γ i-1,j , Γ i,j-1 , Γ i-1,j-1 are all cumulative distance matrices, ρ is the final alignment path, (p1,q1), (p2,q2), (p3,q3), (p n ,q n ) is a series of point pairs of the alignment path;

[0026] ③ Calculate the temporal local difference and inter-axis difference. The temporal local difference is calculated by dividing the alignment path into sub-segments and calculating the average DTW distance of each segment. The process is given by the following formula;

[0027]

[0028] Where, δ i represents the local difference in time, the subscript i represents the i-th subsegment, N i is the number of alignment points in the sub-segment, (p,q∈S i ) means that there are a series of point pairs (p,q) in the i-th sub-segment, d p , t q Respectively represent the dynamic time series and static time series of the subsegment where the point pair (p,q) is located;

[0029] ④ The inter-axis difference is calculated by calculating the difference ratio of each axis in the local alignment, and the axis with external perturbation is determined according to the difference. The process is given by the following formula:

[0030]

[0031] Where, γ κ represents the difference between axes, and the subscript κ represents the κth axis. is the sum of the absolute differences of all aligned points on the current axis, is the sum of the absolute differences across all axes and all alignment points.

[0032] Furthermore, in step 3, the DTW-based adaptive covariance adjustment strategy includes process noise covariance and observation noise covariance:

[0033]

[0034] Where, is the adjusted process noise covariance, Q k-1 is the process noise covariance at time k-1, δ i is the temporal local difference of the i-th sub-segment, δ max is the maximum value of the local difference in historical time, α is the process noise scaling factor, Q base is the baseline process noise covariance; is the adjusted observation noise covariance, R k is the observation noise covariance at time k, γ κ is the inter-axis difference of the κth axis, β is the observation noise scaling factor, is the reference observation noise covariance of the κth axis.

[0035] Furthermore, in step 4, the state variable x of the unscented Kalman filter UKF is defined as k :

[0036]

[0037] Where q0 is the scalar part of the estimated quaternion, q1, q2, q3 are the vector parts of the estimated quaternion; The X, Y, and Z axes are the random errors of the gyroscope, and the Sigma points of the UKF are calculated by unscented transformation:

[0038]

[0039] Where, is the sigma point, are the posterior estimates and their covariance at time k-1, respectively, and λ is the scaling factor;

[0040] UKF Status Forecast and its covariance It is given by:

[0041]

[0042] Where f(·) represents the nonlinear state transfer function, and are the weights of the prior estimated mean and its covariance, is the process noise covariance after adjustment in step 3.

[0043] Furthermore, in step 5, the updating step includes observation value updating and posterior estimation updating, and the observation value updating process is given by the following formula:

[0044]

[0045] Where, represents the predicted observation value, h(·) represents the nonlinear observation transfer function, is the sigma point, represents the predicted observed mean, P zz is the observation-prediction covariance matrix, P xz is the cross-covariance matrix between states and observations, and are the weights of the prior estimated mean and its covariance, is the observed noise covariance after adjustment in step 3;

[0046] Calculate the gain K of the Kalman filter k , and update the posterior estimate and its covariance

[0047]

[0048] Where, are the posterior estimates and their covariances respectively; z k is the observation value, provided by the accelerometer and magnetometer; are the state prediction values ​​and their covariances, respectively.

[0049] Compared with the prior art, the present invention has the following advantages:

[0050] The method of the present invention introduces the DTW algorithm to perform time local difference analysis on the multi-dimensional time series of the IMU, calculates the inter-axis difference of the data of different axes inside the sensor, and dynamically optimizes the covariance of the filter through the time local difference and multi-dimensional inter-axis difference, thereby overcoming the influence of dynamic disturbances, improving the accuracy of attitude estimation, and providing a high-precision and strong robust method for motion attitude estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 is a flow chart of the method of the present invention;

[0052] Figure 2 It is a structural block diagram of the present invention. DETAILED DESCRIPTION

[0053] In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0054] The purpose of the present invention is to provide a DTW-UKF collaborative attitude estimation method for multi-source noise, which overcomes the influence of motion acceleration and ferromagnetic disturbance on attitude estimation by combining DTW and UKF algorithms.

[0055] like Figure 1 and Figure 2 As shown in FIG, a DTW-UKF collaborative attitude estimation method for multi-source noise includes the following steps:

[0056] Step 1: Preprocess the IMU data and construct static time series and dynamic time series based on the preprocessed data.

[0057] Wearable inertial sensors are used to collect athlete posture information, which can be divided into two situations: static and dynamic. The preprocessing process is based on the IMU static reference information to normalize the dynamic data by mean and standard deviation. The process is given by the following formula:

[0058]

[0059] Among them, y norm is the normalized dynamic data, namely angular velocity, acceleration and magnetic field vector; y is the original dynamic data, μ static , σ static are the mean and standard deviation of the static data respectively; a static time series is established based on the mean, which consists of zero angular velocity, gravitational acceleration and geomagnetic field vector.

[0060] The time sliding window is used to split the static and dynamic data in real time, and the static time series and dynamic time series are obtained respectively, which are expressed as T = {t1, t2, t3, ..., t n}, D={d1,d2,d3,...,d m}, the length of the time sliding window is 6 time steps, and the overlap rate of the window is 50%.

[0061] Step 2: Calculate the alignment path of the dynamic time series D and the static time series T using the multidimensional DTW algorithm, and calculate the local temporal difference and inter-axis difference.

[0062] The specific calculation process is as follows:

[0063] ① Define the multidimensional weighted Euclidean distance dist(·) of the DTW algorithm:

[0064]

[0065] Where, ω κ is the weight coefficient of each axis, d i , t j Represent the dynamic time series and static time series of each time window respectively, and Represents the dynamic time series and static time series of each axis of the IMU.

[0066] ② Calculate the alignment path of the dynamic time series D and the static time series T using the DTW algorithm. The process is given by the following formula:

[0067] Γ i,j =dist(d i ,t j )+min(Γ i-1,j ,Γ i,j-1 ,Γ i-1,j-1 ) (3)

[0068] ρ={(p1,q1),(p2,q2),(p3,q3),...,(p n ,q n )} (4)

[0069] Where, Γ i,j , Γ i-1,j , Γ i,j-1 , Γ i-1,j-1 are all cumulative distance matrices, ρ is the final alignment path, (p1,q1), (p2,q2), (p3,q3), (p n ,q n ) is a series of point pairs for the alignment path.

[0070] ③ Calculate the temporal local difference and inter-axis difference. The temporal local difference is calculated by dividing the alignment path into sub-segments and calculating the average DTW distance of each segment. The process is given by the following formula;

[0071]

[0072] Where, δ i represents the local difference in time, the subscript i represents the i-th subsegment, N i is the number of alignment points in the sub-segment, (p,q∈S i ) means that there are a series of point pairs (p,q) in the i-th sub-segment, d p , t qThey represent the dynamic time series and static time series of the subsegment where the point pair (p,q) is located.

[0073] ④ The inter-axis difference is calculated by calculating the difference ratio of each axis in the local alignment, and the axis with external perturbation is determined according to the difference. The process is given by the following formula:

[0074]

[0075] Where, γ κ represents the difference between axes, and the subscript κ represents the κth axis. is the sum of the absolute differences of all aligned points on the current axis, is the sum of the absolute differences across all axes and all alignment points.

[0076] Step 3: Adaptively adjust the noise covariance of the UKF based on the calculated temporal local difference and inter-axis difference.

[0077] The DTW-based adaptive covariance adjustment strategy includes process noise covariance and observation noise covariance:

[0078]

[0079] Where, is the adjusted process noise covariance, Q k-1 is the process noise covariance at time k-1, δ i is the temporal local difference of the i-th sub-segment, δ max is the maximum value of the local difference in historical time, α is the process noise scaling factor, Q base is the baseline process noise covariance; is the adjusted observation noise covariance, R k is the observation noise covariance at time k, γ κ is the inter-axis difference of the κth axis, β is the observation noise scaling factor, is the reference observation noise covariance of the κth axis.

[0080] Step 4: Define the state variables of UKF, generate Sigma point set, and predict the state variables and its covariance

[0081] Define the state variable x of the unscented Kalman filter UKF k :

[0082]

[0083] Where q0 is the scalar part of the estimated quaternion, q1, q2, q3 are the vector parts of the estimated quaternion; The X, Y, and Z axes are the random errors of the gyroscope, and the Sigma points of the UKF are calculated by unscented transformation:

[0084]

[0085] Where, is the sigma point, are the posterior estimates and their covariance at time k-1, and λ is the scaling factor.

[0086] UKF Status Forecast and its covariance It is given by:

[0087]

[0088] Where f(·) represents the nonlinear state transfer function, and are the weights of the prior estimated mean and its covariance, is the process noise covariance after adjustment in step 3.

[0089] Step 5: Update the posterior estimate of UKF and its covariance by combining the observed data.

[0090] The update step includes observation value update and posterior estimate update. The observation value update process is given by the following formula:

[0091]

[0092] Where, represents the predicted observation value, h(·) represents the nonlinear observation transfer function, is the sigma point, represents the predicted observed mean, P zz is the observation-prediction covariance matrix, P xz is the cross-covariance matrix between states and observations, and are the weights of the prior estimated mean and its covariance, is the observation noise covariance after adjustment in step 3.

[0093] Calculate the gain K of the Kalman filter k , and update the posterior estimate and its covariance

[0094]

[0095] Where, are the posterior estimates and their covariances respectively; z k is the observation value, provided by the accelerometer and magnetometer; are the state prediction values ​​and their covariances, respectively.

[0096] Repeat steps 2-5 to obtain the pose estimation at all times.

[0097] The present invention combines DTW and UKF and applies them to the problem of motion attitude estimation. First, the temporal local difference analysis is performed on the multidimensional time series of IMU data, and the inter-axis difference of the data in different axes inside the sensor is calculated. Then, the covariance of the filter is adaptively adjusted based on the temporal local difference and the inter-axis difference. This method can effectively overcome the influence of motion acceleration and ferromagnetic disturbance on the attitude estimation results, and achieve high-precision and strong robust motion attitude estimation.

[0098] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A DTW-UKF collaborative attitude estimation method for multi-source noise, characterized by: The following steps are involved: Step 1: Preprocess the IMU data and construct static time series and dynamic time series based on the preprocessed data; Step 2: Calculate the alignment path of the dynamic time series D and the static time series T using the multidimensional DTW algorithm, and calculate the local temporal difference and inter-axis difference. Step 3: Adaptively adjust the noise covariance of the UKF according to the calculated temporal local difference and inter-axis difference; Step 4: Define the state variables of UKF, generate Sigma point set, and predict the state variables and its covariance Step 5: Update the posterior estimate of UKF and its covariance by combining the observed data; Repeat steps 2-5 to obtain the pose estimation at all times.

2. The DTW-UKF collaborative attitude estimation method for multi-source noise according to claim 1, characterized in that: In step 1, the preprocessing process normalizes the mean and standard deviation of the dynamic data based on the IMU static reference information. The process is given by the following formula: Where y norm is the normalized dynamic data, namely angular velocity, acceleration and magnetic field vector; y is the original dynamic data, μ static , σ static are the mean and standard deviation of the static benchmark information respectively; a static time series is established based on the mean, which consists of zero angular velocity, gravitational acceleration and geomagnetic field vector.

3. The DTW-UKF collaborative attitude estimation method for multi-source noise according to claim 2, characterized in that: In step 1, a time sliding window is used to segment static data and dynamic data in real time, and a static time series and a dynamic time series are obtained, which are represented by T = {t1, t2, t3, ..., t n }, D={d1,d2,d3,...,d m }, the length of the time sliding window is 6 time steps, and the overlap rate of the window is 50%.

4. The DTW-UKF collaborative attitude estimation method for multi-source noise according to claim 1, characterized in that: In step 2, the specific calculation process is as follows: ① Define the multidimensional weighted Euclidean distance dist(·) of the DTW algorithm: Where, ω κ is the weight coefficient of each axis, d i , t j Represent the dynamic time series and static time series of each time window respectively, and Represents the dynamic time series and static time series of each axis of the IMU; ② Calculate the alignment path of the dynamic time series D and the static time series T using the DTW algorithm. The process is given by the following formula: C i,j =dist(d i ,t j )+min(Γ i-1,j ,C i,j-1 ,C i-1,j-1 ) (3) ρ={(p1,q1),(p2,q2),(p3,q3),...,(p n ,q n )} (4) Where, Γ i,j , Γ i-1,j , Γ i,j-1 , Γ i-1,j-1 are all cumulative distance matrices, ρ is the final alignment path, (p1,q1), (p2,q2), (p3,q3), (p n ,q n ) is a series of point pairs of the alignment path; ③ Calculate the temporal local difference and inter-axis difference. The temporal local difference is calculated by dividing the alignment path into sub-segments and calculating the average DTW distance of each segment. The process is given by the following formula; Where, δ i represents the local difference in time, the subscript i represents the i-th subsegment, N i is the number of alignment points in the sub-segment, (p,q∈S i ) means that there are a series of point pairs (p,q) in the i-th sub-segment, d p , t q Respectively represent the dynamic time series and static time series of the subsegment where the point pair (p,q) is located; ④ The inter-axis difference is calculated by calculating the difference ratio of each axis in the local alignment, and the axis with external perturbation is determined according to the difference. The process is given by the following formula: Where, γ κ represents the difference between axes, and the subscript κ represents the κth axis. is the sum of the absolute differences of all aligned points on the current axis, is the sum of the absolute differences across all axes and all alignment points.

5. The DTW-UKF collaborative attitude estimation method for multi-source noise according to claim 1, characterized in that: In step 3, the DTW-based adaptive covariance adjustment strategy includes process noise covariance and observation noise covariance: Where, is the adjusted process noise covariance, Q k-1 is the process noise covariance at time k-1, δ i is the temporal local difference of the i-th sub-segment, δ max is the maximum value of the local difference in historical time, α is the process noise scaling factor, Q base is the baseline process noise covariance; is the adjusted observation noise covariance, R k is the observation noise covariance at time k, γ κ is the inter-axis difference of the κth axis, β is the observation noise scaling factor, is the reference observation noise covariance of the κth axis.

6. The DTW-UKF collaborative attitude estimation method for multi-source noise according to claim 1, characterized in that: In step 4, the state variable x of the unscented Kalman filter UKF is defined as k : Where q0 is the scalar part of the estimated quaternion, q1, q2, q3 are the vector parts of the estimated quaternion; The X, Y, and Z axes are the random errors of the gyroscope, and the Sigma points of the UKF are calculated by unscented transformation: Where, is the sigma point, are the posterior estimates and their covariance at time k-1, respectively, and λ is the scaling factor; UKF Status Forecast and its covariance It is given by: Where f(·) represents the nonlinear state transfer function, and are the weights of the prior estimated mean and its covariance, is the process noise covariance after adjustment in step 3.

7. The DTW-UKF collaborative attitude estimation method for multi-source noise according to claim 1, characterized in that: In step 5, the update step includes observation value update and posterior estimate update. The observation value update process is given by the following formula: Where, represents the predicted observation value, h(·) represents the nonlinear observation transfer function, is the sigma point, represents the predicted observed mean, P zz is the observation-prediction covariance matrix, P xz is the cross-covariance matrix between states and observations, and are the weights of the prior estimated mean and its covariance, is the observed noise covariance after adjustment in step 3; Calculate the gain K of the Kalman filter k , and update the posterior estimate and its covariance Where, are the posterior estimates and their covariances respectively; z k is the observation value, provided by the accelerometer and magnetometer; are the state prediction values ​​and their covariances, respectively.

Images