Exoskeleton control filtering method based on fusion of uwb and imu

Abstract

The application belongs to the technical field of radio positioning and motion control, and specifically discloses a kind of exoskeleton control filtering methods based on UWB and IMU fusion, comprising: deploying UWB base station, UWB label and IMU module;First displacement of knee joint in horizontal direction is measured by UWB label, first forward speed is calculated, second displacement of knee joint in horizontal direction is measured by IMU module, and second forward speed is calculated;State equation and observation equation are constructed, Sigma point is generated and untraceable Kalman filtering iteration is executed, and prediction result is optimized until the confidence of prediction result reaches a preset value.The application applies PDOA algorithm to solve angle, the angle and angular velocity measured by IMU module are fused by untraceable Kalman filtering, the motion direction error caused by single sensor is avoided, and the attitude estimation precision of exoskeleton control is improved.The application is suitable for exoskeleton control.

Description

Technical Field

[0001] This invention belongs to the field of radio positioning and motion control technology, specifically a method for exoskeleton control filtering based on the fusion of UWB and IMU. Background Technology

[0002] An exoskeleton is a wearable device combining mechanical structures, sensors, and a power system, capable of enhancing human mobility or assisting in rehabilitation through biomimetic design. In the application of lower limb passive exoskeletons, accurate motion prediction is crucial for effective assistance. Because different pedestrians walk differently, it is difficult to accurately predict pedestrian movement, especially when using a single sensor. This often leads to discrepancies between the predicted data and the predicted direction, preventing the exoskeleton from providing effective assistance. Furthermore, traditional complementary filtering algorithms lack state estimation and noise modeling, and do not adjust weights based on changes in observations. If the observed data changes abruptly while the filter still calculates with fixed weights, this will cause a shift in the prediction results, resulting in the exoskeleton's assistance being out of sync with the user's actual movement, affecting user safety. Summary of the Invention

[0003] The purpose of this invention is to provide an exoskeleton control filtering method based on the fusion of UWB and IMU. The method uses ultra-wideband (UWB) technology, applies the PDOA algorithm to calculate the angle, and uses the angle and angular velocity measured by the IMU module to fuse them through unscented Kalman filtering. The prediction data is adjusted according to dynamic weights to improve the posture estimation accuracy of exoskeleton control.

[0004] To achieve the above objectives, the present invention employs the following technical methods:

[0005] An exoskeleton control filtering method based on UWB and IMU fusion includes:

[0006] S1. Install the UWB base station in the center of the abdomen of the exoskeleton, at the same level as the hip joint; install the UWB tag and IMU module above the knee joint of the exoskeleton.

[0007] S2. Within the same gait cycle, the first displacement of the knee joint in the horizontal direction is measured by the UWB tag, and the first forward velocity is calculated. The second displacement of the knee joint in the horizontal direction is measured by the IMU module, and the second forward velocity is calculated.

[0008] S3. Based on the first and second forward velocities, construct the state equation and observation equation, perform unscented Kalman filtering, generate Sigma points, and execute unscented Kalman filtering iterations. Compare the angular velocity and acceleration obtained from the IMU module to optimize the prediction results until the confidence level of the prediction results reaches the preset value.

[0009] As a limitation, step S2 specifically includes:

[0010] S21. Establish a three-dimensional coordinate system with the UWB base station as the origin, the direction of human forward movement as the X-axis, the direction from the UWB base station to the hip joint as the Y-axis, and the vertical direction as the Z-axis.

[0011] S22. Calculate the distance L1 between the UWB tag and the hip joint, the distance L2 between the UWB tag and the UWB base station, and the angle θ between the line connecting the UWB tag and the UWB base station and the horizontal plane using the PDOA algorithm. UWB ;

[0012] S23. Calculate the angle θ between the line connecting the IMU module and the hip joint and the horizontal plane using a state machine. IMU , as well as the angular velocity and acceleration measured by the IMU module;

[0013] S24. Project the distance L1 between the UWB tag and the hip joint, and the distance L2 between the UWB tag and the UWB base station, onto the plane containing the X-axis and Y-axis, i.e.:

[0014] ,

[0015] ,

[0016] in, This is the projected length of the UWB label relative to the hip joint distance L1. The projected length of the distance L2 between the UWB tag and the UWB base station;

[0017] S25. Calculate the projection length L1 between the UWB label and the hip joint. Distance between UWB tag and UWB base station (L2 projection length) The included angle η, that is:

[0018] ,

[0019] in, The distance between the hip joint and the UWB base station;

[0020] The first displacement of the knee joint in the horizontal direction for:

[0021] ;

[0022] The first forward velocity of the knee joint in the horizontal direction for:

[0023] ,

[0024] Where t is a gait period and the first forward velocity vector is... After normalization, the first forward velocity normalized vector is obtained. ,like , ,like , ;

[0025] The second displacement of the knee joint in the horizontal direction for:

[0026] ;

[0027] The second forward velocity of the knee joint in the horizontal direction for:

[0028] ,

[0029] Second forward velocity vector .

[0030] As a further limitation, step S23 also includes taking 12 consecutive sample values ​​of the angular velocity and acceleration data obtained from the IMU module and performing an arithmetic average operation, i.e., arithmetic average filtering, to obtain the arithmetic average filtered angular velocity and acceleration.

[0031] As a constraint, the state equation constructed in step S3 is as follows:

[0032] ,

[0033] in, Let be the forward velocity at time k. Let the forward speed be at time k-1. Let A be the first forward velocity vector, and let A be the scaling factor of the first forward velocity vector. Let A be the magnitude of the second forward velocity vector, and B be the scaling factor of the magnitude of the second forward velocity vector, where A < B and A + B = 1. This is the normalized vector of the first forward velocity. This represents the process noise at time k-1.

[0034] The constructed observation equation is as follows:

[0035] ,

[0036] in, Let be the observed forward velocity at time k. Let k be the first forward velocity at time k. The second forward velocity at time k is... Let be the observation noise at time k.

[0037] As a further limitation, the unscented Kalman filtering in step S3 specifically involves:

[0038] Generate 2n+1 Sigma points and perform unscented Kalman filtering. When the current state is When the forward velocity is at time kl, the updated state estimation error covariance matrix is: , Where γ is a scaling factor for generating Sigma points, n is the state dimension (n=2), and λ is the scaling parameter. α is a parameter controlling the distribution range of the Sigma points, and κ is a minor scaling parameter under a Gaussian distribution. , The Cholesky decomposition of the error covariance matrix satisfies ;

[0039] The matrix of Sigma points is ,in , For the initial state estimation of forward velocity, For the initial state estimation of the first forward velocity, For the initial state estimation of the second forward velocity, the remaining Sigma points , Where 1 ≤ i ≤ 4, and i is an integer. Represents a matrix After performing Cholesky decomposition, take the i-th column; for Sigma points Propagation is performed using state equations to obtain the predicted Sigma point at time k. , ,in, This is the Sigma point at time k-1;

[0040] Then, use mean weights. Covariance weights The predicted forward velocity at time k is obtained by taking a weighted average of the predicted Sigma points. , Predicting the error covariance at time k Where Q is the process noise covariance matrix; the predicted Sigma point is projected onto the observation space through the observation equation to obtain the velocity components of the observed Sigma point at time k. , ,in, The first velocity component of the Sigma point is observed at time k. For the second velocity component observed at point Sigma at time k, calculate the predicted observation mean. and observation covariance , , Where R is the observation noise covariance matrix; and the cross covariance between state and observation is... The calculation formula is ;

[0041] Finally, calculate the Kalman gain at time k. By updating the state estimate and error covariance matrix using Kalman gain, one unscented Kalman iteration is completed. , , To update the forward velocity at time k after estimating the state, This is the error covariance matrix at time k after updating the state estimate.

[0042] The beneficial effects achieved by this invention, due to the adoption of the above-described solution, compared with the prior art, are as follows:

[0043] This invention provides an exoskeleton control filtering method based on the fusion of UWB and IMU. By using ultra-wideband (UWB) technology and applying the PDOA algorithm to calculate the angle, the angle and angular velocity measured by the IMU module are fused by unscented Kalman filtering, which avoids motion direction errors caused by a single sensor. The predicted data is adjusted according to dynamic weights, thereby improving the attitude estimation accuracy of exoskeleton control.

[0044] This invention is applicable to the control of exoskeletons. Attached Figure Description

[0045] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0046] Figure 1 This is a flowchart of an exoskeleton control filtering method based on the fusion of UWB and IMU according to an embodiment of the present invention;

[0047] Figure 2 This is a schematic diagram of the exoskeleton in a three-dimensional coordinate system according to an embodiment of the present invention;

[0048] Figure 3 To compare the fused data obtained by the exoskeleton control filtering method based on UWB and IMU fusion in this embodiment of the invention with the data measured by a single UWB tag and a single IMU module;

[0049] In the diagram: 1. UWB base station; 2. Hip joint; 3. Knee joint; 4. UWB tag; 5. IMU module. Detailed Implementation

[0050] The present invention will be further described below with reference to the embodiments. However, those skilled in the art should understand that the present invention is not limited to the following embodiments. Any improvements and equivalent changes made based on the specific embodiments of the present invention are within the scope of protection of the claims of the present invention.

[0051] Example

[0052] An exoskeleton control filtering method based on UWB and IMU fusion, such as Figure 1 As shown, it includes:

[0053] S1. Install the UWB base station in the center of the abdomen of the exoskeleton, at the same level as the hip joint; install the UWB tag and IMU module above the knee joint of the exoskeleton; the UWB base station and UWB tag use the ULM3 kit, and the IMU module model is JY61P.

[0054] S2. Within the same gait cycle, the first horizontal displacement of the knee joint is measured using a UWB tag, and the first forward velocity is calculated. The second horizontal displacement of the knee joint is measured using an IMU module, and the second forward velocity is calculated. Specifically, this includes:

[0055] S21. Establish a three-dimensional coordinate system with UWB base station 1 as the origin, the direction of human forward movement as the X-axis, the direction from UWB base station 1 to hip joint 2 as the Y-axis, and the vertical direction as the Z-axis, as follows: Figure 2 As shown;

[0056] S22. Calculate the distance L1 between UWB tag 4 and hip joint 2, the distance L2 between UWB tag 4 and UWB base station 1, and the angle θ between the line connecting UWB tag 4 and UWB base station 1 and the horizontal plane using the PDOA algorithm. UWB ;

[0057] S23. Calculate the angle θ between the line connecting IMU module 5 and hip joint 2 and the horizontal plane using a state machine. IMU The angular velocity and acceleration measured by IMU module 5 are used to perform an arithmetic average operation on 12 consecutive sample values ​​of the obtained angular velocity and acceleration data, i.e., arithmetic average filtering, to obtain the arithmetic average filtered angular velocity and acceleration.

[0058] S24. Project the distance L1 between UWB tag 4 and hip joint 2, and the distance L2 between UWB tag 4 and UWB base station 1 onto the plane containing the X-axis and Y-axis, that is:

[0059] ,

[0060] ,

[0061] in, The projected length of the distance L1 between UWB tag 4 and hip joint 2. The projected length of the distance L2 between UWB tag 4 and UWB base station 1;

[0062] S25. Calculate the projected length L1 between UWB tag 4 and hip joint 2. The distance L2 projection length between UWB tag 4 and UWB base station 1 The included angle η, that is:

[0063] ,

[0064] in, The distance between hip joint 2 and UWB base station 1;

[0065] The first displacement of knee joint 3 in the horizontal direction for:

[0066] ;

[0067] The first forward velocity of the knee joint 3 in the horizontal direction for:

[0068] ,

[0069] Where t is a gait period and the first forward velocity vector is... After normalization, the first forward velocity normalized vector is obtained. ,like , ,like , ;

[0070] The second displacement of the knee joint in the horizontal direction for:

[0071] ;

[0072] The second forward velocity of the knee joint in the horizontal direction for:

[0073] ,

[0074] Second forward velocity vector .

[0075] S3. Based on the first and second forward speeds, construct the state equation and observation equation, perform unscented Kalman filtering, generate Sigma points, and execute unscented Kalman filtering iterations. Convert the data after unscented Kalman filtering into acceleration and angular velocity, and compare them with the angular velocity and acceleration after arithmetic average filtering in step S23. Compare the closeness of the two to the real gait data collected by the motor encoder in the exoskeleton. By continuously performing unscented Kalman filtering iterations, optimize the prediction results until the prediction results are closer to the real gait and the confidence level of the prediction results reaches a preset value. In this embodiment, the preset confidence level is 95%.

[0076] The constructed state equation is as follows:

[0077] ,

[0078] in, Let be the forward velocity at time k. Let the forward speed be at time k-1. Let A be the first forward velocity vector, and let A be the scaling factor of the first forward velocity vector. Let A be the magnitude of the second forward velocity vector, and B be the scaling factor of the magnitude of the second forward velocity vector, where A < B and A + B = 1. This is the normalized vector of the first forward velocity. This represents the process noise at time k-1.

[0079] The constructed observation equation is as follows:

[0080] ,

[0081] in, Let be the observed forward velocity at time k. Let k be the first forward velocity at time k. The second forward velocity at time k is... Let be the observation noise at time k.

[0082] Construct the initial error covariance matrix Process noise covariance matrix Observation noise covariance matrix Initial state estimation of forward velocity .

[0083] The process of unscented Kalman filtering is as follows:

[0084] Generate 2n+1 Sigma points and perform unscented Kalman filtering. When the current state is When the forward velocity is at time kl, the updated state estimation error covariance matrix is: , Where γ is a scaling factor for generating Sigma points, n is the state dimension (n=2), and λ is the scaling parameter. α is a parameter controlling the distribution range of the Sigma points, and κ is a minor scaling parameter under a Gaussian distribution. Let α = 0.2. The Cholesky decomposition of the error covariance matrix satisfies ;

[0085] The matrix of Sigma points is ,in , For the initial state estimation of forward velocity, For the initial state estimation of the first forward velocity, For the initial state estimation of the second forward velocity, the remaining Sigma points , Where 1 ≤ i ≤ 4, and i is an integer. Represents a matrix After performing Cholesky decomposition, take the i-th column; for Sigma points Propagation is performed using state equations to obtain the predicted Sigma point at time k. , ,in, This is the Sigma point at time k-1;

[0086] Then, use mean weights. Covariance weights The predicted forward velocity at time k is obtained by taking a weighted average of the predicted Sigma points. , Predicting the error covariance at time k Where Q is the process noise covariance matrix; the predicted Sigma point is projected onto the observation space through the observation equation to obtain the velocity components of the observed Sigma point at time k. , ,in, The first velocity component of the Sigma point is observed at time k. For the second velocity component observed at point Sigma at time k, calculate the predicted observation mean. and observation covariance , , Where R is the observation noise covariance matrix; and the cross covariance between state and observation is... The calculation formula is ;

[0087] Finally, calculate the Kalman gain at time k. By updating the state estimate and error covariance matrix using Kalman gain, one unscented Kalman iteration is completed. , , To update the forward velocity at time k after estimating the state, This is the error covariance matrix at time k after updating the state estimate.

[0088] This embodiment employs an exoskeleton control filtering method based on UWB and IMU fusion. The angle calculated using the PDOA algorithm is fused with the angle and angular velocity values ​​measured by the IMU using an unscented Kalman filter. This avoids motion direction errors caused by a single sensor and improves the posture estimation accuracy of the exoskeleton control. Figure 3 As shown, the fused data is closer to the actual gait data than the data measured by a single UWB tag and a single IMU module.

Claims

1. An exoskeleton control filtering method based on UWB and IMU fusion, characterized in that, include: S1. Install the UWB base station in the center of the abdomen of the exoskeleton, and place it at the same level as the hip joint; The UWB tag and IMU module are mounted above the knee joint of the exoskeleton; S2. Within the same gait cycle, the first displacement of the knee joint in the horizontal direction is measured by the UWB tag, and the first forward velocity is calculated. The second displacement of the knee joint in the horizontal direction is measured by the IMU module, and the second forward velocity is calculated. S3. Based on the first and second forward velocities, construct the state equation and observation equation, perform unscented Kalman filtering, generate Sigma points and perform unscented Kalman filtering iteration, compare with the angular velocity and acceleration obtained by the IMU module, optimize the prediction results until the confidence of the prediction results reaches the preset value. The constructed state equation is as follows: ， in, Let be the forward velocity at time k. Let the forward speed be at time k-1. Let A be the first forward velocity vector, and let A be the scaling factor of the first forward velocity vector. Let A be the magnitude of the second forward velocity vector, and B be the scaling factor of the magnitude of the second forward velocity vector, where A < B and A + B = 1. This is the normalized vector of the first forward velocity. This represents the process noise at time k-1. The constructed observation equation is as follows: ， in, Let be the observed forward velocity at time k. Let k be the first forward velocity at time k. The second forward velocity at time k is... Let be the observation noise at time k.

2. The exoskeleton control filtering method based on UWB and IMU fusion according to claim 1, characterized in that, Step S2 specifically includes: S21. Establish a three-dimensional coordinate system with the UWB base station as the origin, the direction of human forward movement as the X-axis, the direction from the UWB base station to the hip joint as the Y-axis, and the vertical direction as the Z-axis. S22. Calculate the distance L1 between the UWB tag and the hip joint, the distance L2 between the UWB tag and the UWB base station, and the angle θ between the line connecting the UWB tag and the UWB base station and the horizontal plane using the PDOA algorithm. UWB ; S23. Calculate the angle θ between the line connecting the IMU module and the hip joint and the horizontal plane using a state machine. IMU , as well as the angular velocity and acceleration measured by the IMU module; S24. Project the distance L1 between the UWB tag and the hip joint, and the distance L2 between the UWB tag and the UWB base station, onto the plane containing the X-axis and Y-axis, i.e.: , ， in, This is the projected length of the UWB label relative to the hip joint distance L1. The projected length of the distance L2 between the UWB tag and the UWB base station; S25. Calculate the projection length L1 between the UWB label and the hip joint. Distance between UWB tag and UWB base station (L2 projection length) The included angle η, that is: ， in, The distance between the hip joint and the UWB base station; The first displacement of the knee joint in the horizontal direction for: ； The first forward velocity of the knee joint in the horizontal direction for: ， Where t is a gait period and the first forward velocity vector is... After normalization, the first forward velocity normalized vector is obtained. ,like , ,like , ; The second displacement of the knee joint in the horizontal direction for: ； The second forward velocity of the knee joint in the horizontal direction for: ， Second forward velocity vector .

3. The exoskeleton control filtering method based on UWB and IMU fusion according to claim 2, characterized in that, Step S23 further includes taking 12 consecutive sample values ​​of the angular velocity and acceleration data obtained from the IMU module and performing an arithmetic average operation, i.e., arithmetic average filtering, to obtain the arithmetic average filtered angular velocity and acceleration.

4. The exoskeleton control filtering method based on UWB and IMU fusion according to claim 1, characterized in that, The unscented Kalman filtering in step S3 is specifically performed as follows: Generate 2n+1 Sigma points and perform unscented Kalman filtering. When the current state is When the forward velocity is at time kl, the updated state estimation error covariance matrix is: , Where γ is a scaling factor for generating Sigma points, n is the state dimension (n=2), and λ is the scaling parameter. α is a parameter controlling the distribution range of the Sigma points, and κ is a minor scaling parameter under a Gaussian distribution. , The Cholesky decomposition of the error covariance matrix satisfies ; The matrix of Sigma points is ,in , For the initial state estimation of forward velocity, For the initial state estimation of the first forward velocity, For the initial state estimation of the second forward velocity, the remaining Sigma points , Where 1 ≤ i ≤ 4, and i is an integer. Represents a matrix After performing Cholesky decomposition, take the i-th column; for Sigma points Propagation is performed using state equations to obtain the predicted Sigma point at time k. , ,in, This is the Sigma point at time k-1; Then, use mean weights. Covariance weights The predicted forward velocity at time k is obtained by taking a weighted average of the predicted Sigma points. , Predicting the error covariance at time k Where Q is the process noise covariance matrix; the predicted Sigma point is projected onto the observation space through the observation equation to obtain the velocity components of the observed Sigma point at time k. , ,in, The first velocity component of the Sigma point is observed at time k. For the second velocity component observed at point Sigma at time k, calculate the predicted observation mean. and observation covariance , , Where R is the observation noise covariance matrix; and the cross covariance between state and observation is... The calculation formula is ; Finally, calculate the Kalman gain at time k. By updating the state estimate and error covariance matrix using Kalman gain, one unscented Kalman iteration is completed. , , To update the forward velocity at time k after estimating the state, This is the error covariance matrix at time k after updating the state estimate.

Images