Dynamic motion trajectory extraction method based on kalman filtering and duplicate data screening

Abstract

The application provides a dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening, which comprises the following steps: installing IMU on a plurality of key motion joints of a dynamic living body, generating an initial trajectory data set through data collected by the IMU; setting a state space model based on the measurement data in the initial trajectory data set and Euler angles, and performing noise compensation on the initial trajectory data set through a Kalman filter; optimizing the initial trajectory data set after Kalman filtering through a repeated data screening (RDS) algorithm to obtain an optimized trajectory data set; and calculating the joint axis direction through the optimized trajectory data set, and constructing the dynamic motion trajectory of the dynamic living body according to the calculated joint axis direction and the angular velocity data after Kalman filtering. The method combines the Kalman filtering algorithm and the repeated data screening algorithm to extract the trajectory, and significantly improves the robustness and accuracy of the dynamic motion trajectory extraction of the living body.

Description

Technical Field

[0001] The present application relates to the technical field of motion trajectory extraction, and in particular to a dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening. Background Art

[0002] At present, the motion trajectory extraction of living organisms has been widely used in fields such as exoskeleton robot control, sports rehabilitation monitoring, and wearable device trajectory tracking. Many application products need to extract the motion trajectory of living organisms in dynamic scenes to implement their operation control strategies.

[0003] In related technologies, an inertial measurement unit (IMU) is usually used to extract the motion trajectory of a dynamic living organism. Relevant data is collected by the IMU and processed by a mathematical model to finally obtain the motion trajectory of the living organism.

[0004] However, in practical applications, when performing high-precision motion trajectory extraction in complex environments, since the extraction of motion trajectories of dynamic life forms is usually affected by factors such as sensor noise, data deviation, and environmental interference, the methods in the above-mentioned related technologies are difficult to achieve effective extraction and processing of low signal-to-noise ratio data. In addition, since the data of the inertial measurement unit is easily affected by sensor errors (such as acceleration drift and gyroscope integration error), the motion trajectory extraction results have large uncertainty, and the data acquisition accuracy is unstable, especially in long-term movement. Errors will accumulate. The above-mentioned reasons lead to the poor accuracy of the motion trajectory extraction results of dynamic life forms in the related technologies. Moreover, when facing different dynamic scenes, the trajectory extraction algorithms in the related technologies are often sensitive to the initial parameters, easily fall into local optimal solutions, and lack the ability to robustly process complex environments.

[0005] Therefore, how to obtain high-precision and high-robustness dynamic motion trajectory extraction results of living organisms has become an urgent problem that needs to be solved. Summary of the Invention

[0006] The present application aims to solve one of the technical problems in the related art at least to a certain extent.

[0007] To this end, the first purpose of this application is to propose a dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening. This method combines the Kalman filtering algorithm with the repeated data screening algorithm for trajectory extraction, which significantly improves the robustness and accuracy of the dynamic motion trajectory extraction of living organisms, and can be applied to the motion trajectory extraction of living organisms in a variety of dynamic scenarios.

[0008] The second objective of this application is to propose a dynamic motion trajectory extraction system based on Kalman filtering and repeated data screening.

[0009] A third object of the present application is to provide a non-transitory computer-readable storage medium.

[0010] To achieve the above objectives, the first aspect of the present application is to propose a dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening, the method comprising the following steps:

[0011] An inertial measurement unit (IMU) is installed at each of the key motion joints of the dynamic life form, and the three-axis acceleration and three-axis angular velocity of the corresponding key motion joints during motion are collected by the IMU to generate an initial trajectory data set;

[0012] Setting a state-space model based on the measurement data and Euler angles in the initial trajectory dataset, and performing noise compensation on the initial trajectory dataset using a Kalman filter based on the state-space model;

[0013] The initial trajectory data set after Kalman filtering is optimized by a repeated data screening RDS algorithm to obtain an optimized trajectory data set, wherein the RDS algorithm uses the predicted joint axis direction vector to iteratively screen the sample data in the initial trajectory data set after Kalman filtering;

[0014] The joint axis direction is calculated using the optimized trajectory data set, and the dynamic motion trajectory of the dynamic life form is constructed based on the calculated joint axis direction and the angular velocity data after Kalman filtering.

[0015] Optionally, in one embodiment of the present application, the state space model is set based on the measurement data and Euler angles in the initial trajectory data set, including: constructing a state vector based on the Euler angles and the angular velocity of the Euler angles, and constructing a measurement vector based on the three-axis acceleration and three-axis angular velocity collected by the IMU; based on the state space model, the initial trajectory data set is noise compensated through a Kalman filter, including: constructing a state transfer equation based on the state vector, and constructing a measurement equation based on the measurement vector; based on the state transfer equation and the measurement equation, iteratively performing the prediction process and the update process in the Kalman filter algorithm to suppress the noise and angular velocity drift in the initial trajectory data set.

[0016] Optionally, in one embodiment of the present application, the initial trajectory data set after Kalman filtering is optimized by repeated data screening RDS algorithm, including: constructing a residual function, wherein the residual function is used to calculate the difference between each of the sample data and the multiple predicted joint axis direction vectors; setting the initial sample capacity and the sample removal amount for each round of screening; in each round of screening, calculating the residual value of each current sample data by the residual function, removing the sample data with the largest residual value before the sample removal amount, and calculating the residual mean of each sample data remaining after removal; when the residual mean corresponding to the current round of screening process is less than the residual mean corresponding to the previous round of screening process, or the number of remaining sample data is less than a preset threshold, the iterative screening process is ended.

[0017] Optionally, in one embodiment of the present application, calculating the joint axis direction using the optimized trajectory dataset includes: obtaining roll angle data and pitch angle data in the optimized trajectory dataset, and calculating the joint axis direction based on the obtained angle data using the following formula:

[0018] j=[cos(φ i )cos(θ i ),sin(φ i )cos(θ i ),sin(θ i )]

[0019] Among them, φ i represents the roll angle at the i-th moment, θ i represents the pitch angle at the i-th moment, and j is a three-dimensional unit vector representing the direction of the joint axis; constructing the dynamic motion trajectory of the dynamic life body includes: calculating the product of the joint axis direction and the corresponding angular velocity data, integrating the product, and obtaining the dynamic motion trajectory of the moving joint in three-dimensional space.

[0020] Optionally, in one embodiment of the present application, after constructing the dynamic motion trajectory of the dynamic life form, it also includes: storing the dynamic motion trajectory data in segments according to changes in the motion state; compressing each segment of the dynamic motion trajectory, screening out key data points of the trajectory, and gradually controlling the exoskeleton robot according to the key data points of the trajectory of each segment of the dynamic motion trajectory.

[0021] Optionally, in one embodiment of the present application, the trajectory key data points include joint angles and joint angular velocities, and the exoskeleton robot is gradually controlled according to the trajectory key data points of each dynamic motion trajectory, including: initializing the control system of the exoskeleton robot and loading the trajectory key data points of each dynamic motion trajectory; controlling the exoskeleton robot to perform corresponding operations according to the joint angles and the joint angular velocities of each dynamic motion trajectory in time sequence; receiving feedback data of the exoskeleton robot in real time during the operation of the exoskeleton robot, comparing the feedback data with the dynamic motion trajectory data, and adjusting the action of the exoskeleton robot according to the comparison result.

[0022] Optionally, in one embodiment of the present application, after the exoskeleton robot is gradually controlled, it also includes: storing the actual execution data of the exoskeleton robot in different trajectory segments; evaluating the actual execution data of the different trajectory segments, and optimizing the data processing strategy of the dynamic motion trajectory and the control strategy of the exoskeleton robot based on the evaluation results.

[0023] To achieve the above objectives, the second aspect of the present application further proposes a dynamic motion trajectory extraction system based on Kalman filtering and repeated data screening, comprising the following modules:

[0024] An acquisition module is used to install inertial measurement units (IMUs) at multiple key motion joints of a dynamic life form, and to collect the three-axis acceleration and three-axis angular velocity of the corresponding key motion joints during motion through the IMUs to generate an initial trajectory data set;

[0025] a filtering module, configured to set a state-space model based on the measurement data and Euler angles in the initial trajectory dataset, and perform noise compensation on the initial trajectory dataset using a Kalman filter based on the state-space model;

[0026] a screening module, configured to optimize the initial trajectory data set after Kalman filtering by using a repeated data screening RDS algorithm to obtain an optimized trajectory data set, wherein the RDS algorithm uses the predicted joint axis direction vector to iteratively screen the sample data in the initial trajectory data set after Kalman filtering;

[0027] The extraction module is used to calculate the joint axis direction through the optimized trajectory data set, and construct the dynamic motion trajectory of the dynamic life body according to the calculated joint axis direction and the angular velocity data after Kalman filtering.

[0028] In order to implement the above-mentioned embodiments, the third aspect embodiment of the present application also proposes a non-temporary computer-readable storage medium on which a computer program is stored. When the computer program is executed by the processor, the dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening in the above-mentioned first aspect is implemented.

[0029] The technical solutions provided by the embodiments of the present application bring at least the following beneficial effects: The present application first dynamically corrects the motion model using a Kalman filter, effectively reducing the impact of sensor noise and integral error on trajectory extraction results and achieving dynamic filtering compensation. The collected raw data is then iteratively filtered through a repeated data screening algorithm, effectively eliminating high-error data points, which helps enhance the accuracy of trajectory estimation. Thus, the present application combines the Kalman filter algorithm with the repeated data screening algorithm for trajectory extraction, significantly improving the robustness and accuracy of dynamic motion trajectory extraction for living organisms. Furthermore, the present application can provide stable trajectory estimation results in different dynamic scenarios, adapting to the motion patterns of different living organisms, overcoming the sensitivity of initial conditions to optimization results, enhancing the robustness of dynamic motion trajectory extraction, and being applicable to the extraction of living organism motion trajectories in a variety of dynamic scenarios. As a result, the present application improves the accuracy, robustness, and reliability of dynamic motion trajectory extraction for living organisms, enriches the applicable scenarios of the motion trajectory extraction algorithm, and improves the efficiency of motion trajectory extraction, providing effective technical support for applications such as exoskeleton robots and rehabilitation training equipment.

[0030] Additional aspects and advantages of the present invention will be set forth in part in the description which follows and, in part, will be obvious from the description which follows, or may be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0032] Figure 1 A flowchart of a dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening proposed in an embodiment of the present application;

[0033] Figure 2 This is a flow chart of a trajectory data noise compensation method based on a Kalman filter proposed in an embodiment of the present application;

[0034] Figure 3 This is a flowchart of a trajectory data optimization method based on the RDS algorithm proposed in an embodiment of the present application;

[0035] Figure 4 A flow chart of a control method for an exoskeleton robot proposed in an embodiment of the present application;

[0036] Figure 5 This is a structural diagram of a dynamic motion trajectory extraction system based on Kalman filtering and repeated data screening proposed in an embodiment of the present application. DETAILED DESCRIPTION

[0037] The following describes embodiments of the present invention in detail, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and are not to be construed as limiting the present invention.

[0038] It should be noted that in the relevant embodiments, when extracting dynamic motion trajectories through the inertial measurement unit IMU, the following methods are mainly relied upon: First, traditional optimization methods, such as the common Gauss-Newton method (GN) is used to estimate dynamic alignment and motion trajectories. Second, the complementary filter (CF) algorithm is used to improve the accuracy of motion trajectory extraction. Third, data preprocessing and constraint models are used to preprocess the IMU data (such as low-pass filtering, sampling frequency alignment, etc.), or a kinematic constraint model is introduced to reduce data errors to a certain extent.

[0039] However, the solution of extracting motion trajectories through IMU data in related embodiments still has the following shortcomings in complex application scenarios:

[0040] First, robustness to noise and bias is insufficient. Existing methods have limited ability to process high-noise data. In particular, bias noise contained in the noise can significantly affect trajectory extraction results. For example, in existing Gauss-Newton optimization methods, randomly initialized parameters can lead to local optimal solutions, making the results highly random.

[0041] Second, the ability to adapt to dynamic scenarios is weak. Existing technologies have difficulty effectively handling diverse motion patterns (such as knee bending and complex trajectory changes). Especially in application scenarios with high real-time requirements, traditional methods are easily affected by sampling errors and time accumulation drift.

[0042] Third, sensitivity to initial conditions: Traditional optimization algorithms such as the Gauss-Newton method are highly dependent on initial conditions and lack a systematic screening mechanism for high-noise data, which ultimately leads to unstable trajectory estimation results.

[0043] Fourth, data fusion is imperfect. While existing filtering techniques can compensate for sensor errors to a certain extent, they still cannot guarantee system robustness in high-noise and low-signal-to-noise-ratio environments. Furthermore, existing methods often fail to fully utilize strategies that combine data screening and optimization, thus failing to achieve refined processing of noisy data.

[0044] To this end, this application proposes a dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening to improve the robustness and accuracy of motion trajectory extraction.

[0045] The following describes a dynamic motion trajectory extraction method and system based on Kalman filtering and repeated data screening proposed in an embodiment of the present application with reference to the accompanying drawings.

[0046] Figure 1 This is a flow chart of a dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening proposed in an embodiment of the present application, such as Figure 1 As shown, the method includes the following steps:

[0047] In step S101, inertial measurement units (IMUs) are installed at multiple key motion joints of a dynamic life form, and the three-axis acceleration and three-axis angular velocity of the corresponding key motion joints during motion are collected by the IMUs to generate an initial trajectory data set.

[0048] Specifically, IMUs are used to collect data from dynamic life forms and create a trajectory dataset. An inertial measurement unit (IMU) is installed at each of the life form's key joints (such as the knee and hip joints). This sensor collects the three-axis acceleration and three-axis angular velocity of the life form during movement, and simultaneously records the timestamps to construct a raw motion trajectory dataset.

[0049] For example, the IMU measurement data includes the angular velocity matrix ω = [ω x ,ω y ,ω z ] T And the acceleration matrix a=[a x ,a y ,a z ] T ; Through time synchronization and data interpolation, the sampling frequency is unified to f s , and construct the trajectory data matrix X 3×n , where n is the number of time sample points, X 3×n Represents the three-axis angular velocity or acceleration data at each time point.

[0050] Step S102 : setting a state space model based on the measurement data and Euler angles in the initial trajectory data set, and performing noise compensation on the initial trajectory data set through a Kalman filter based on the state space model.

[0051] Specifically, this application uses a Kalman filter to perform noise compensation on the collected motion trajectory data. To eliminate sensor noise and drift, this application introduces a Kalman filter (KF) algorithm to optimize the raw IMU data. The Kalman filter can process acceleration integration errors, filter out sensor noise, and dynamically correct the motion trajectory.

[0052] In order to more clearly illustrate the specific implementation process of the Kalman filter on the initial trajectory data set in the present application, a filtering method proposed in an embodiment of the present application is exemplified below.

[0053] Figure 2 This is a flow chart of a trajectory data noise compensation method based on a Kalman filter proposed in an embodiment of the present application, such as Figure 2 As shown, the method includes the following steps:

[0054] Step S201: construct a state vector based on the Euler angles and the angular velocity of the Euler angles, and construct a measurement vector based on the three-axis acceleration and three-axis angular velocity collected by the IMU.

[0055] Specifically, the present application first sets the state space model in the Kalman filter algorithm based on the measurement data and Euler angles in the acquired initial trajectory data set. The state space model includes a state vector and a measurement vector. The constructed state vector is: Among them, the Euler angles are divided into φ, θ, and ψ, which are roll angle (Roll), pitch angle (Pitch) and yaw angle (Yaw), which are used to describe the three-dimensional posture of the object. are the angular velocities of the corresponding Euler angles, which represent the rate of change of the Euler angles over time. The constructed measurement vector is: k =[ω x ,ω y ,ω z ,a x ,a y ,a z ] T , the various parameters in the formula are the three-axis acceleration and three-axis angular velocity collected in step S101.

[0056] Step S202: constructing a state transfer equation based on the state vector, and constructing a measurement equation based on the measurement vector.

[0057] Specifically, steps S202 and S203 are based on the set state space model, and the Kalman filter performs a correlation filtering algorithm to compensate the noise of the initial trajectory data set. This step first constructs the state transition equation shown in the following formula based on the state vector:

[0058] x k+1 =Fx k +w k

[0059] Among them, F is the state transfer matrix, w k is the process noise, x k+1 is the state vector at the next moment.

[0060] Then, based on the measurement vector, the measurement equation shown in the following formula is constructed:

[0061] z k =Hx k +v k

[0062] Where H is the measurement matrix, v k To measure noise.

[0063] Step S203 : Based on the state transfer equation and the measurement equation, the prediction process and the update process in the Kalman filter algorithm are iterated to suppress the noise and angular velocity drift in the initial trajectory data set.

[0064] Specifically, in the Kalman filter, the prediction and update phases continuously predict and correct state estimates, enabling the system to efficiently and accurately estimate its state in a dynamic environment. In the prediction phase, the current state is predicted based on the system's dynamic model and the previous estimate. This phase does not rely on any new observations; it uses only previous information (state and covariance) to make predictions.

[0065] When executing the prediction process, the state prediction is performed first, and the state estimation value at the current moment is calculated by the estimated state at the previous moment and the state transfer matrix F. It can be calculated using the following formula:

[0066]

[0067] in, is the state estimate (predicted state) at the current time k, that is, the value predicted based on the state at the previous time and the model. F is the state transition matrix, which describes the transition relationship from time k-1 to time k. It is the state estimate of the previous moment k-1. k is the control input (if the system has external control input), u k is the control vector, and B is the control input matrix.

[0068] Then, we perform covariance prediction. The covariance matrix P reflects the uncertainty of the estimate. In the prediction phase, we use the following prediction formula to calculate the covariance at the current moment:

[0069] P k|k-1 =FP k-1|k-1 F T +Q

[0070] Among them, P k|k-1 is the prediction covariance of the current moment k, reflecting the uncertainty of the prediction value. k-1|k-1 is the estimated covariance at the previous moment. Q is the process noise covariance matrix, which describes the changes caused by the uncertainty of the system itself or external disturbances.

[0071] Further, an update process is performed.

[0072] First, the Kalman gain is calculated. k Used to balance the proportion of predicted value and actual measured value. The calculation formula of Kalman gain is:

[0073] K k =P k|k-1 H T (HP k|k-1 H T +R) -1

[0074] Among them, K k is the Kalman gain, which determines the degree of fusion between the predicted and measured values. H is the measurement matrix, which describes the mapping from state space to measurement space. R is the measurement noise covariance matrix, which reflects the noise level of the measurement data. A larger Kalman gain indicates a higher confidence level in the measured values ​​and a smaller contribution of the predicted values ​​to the final estimate. Conversely, a smaller Kalman gain indicates an unreliable measured value and a greater contribution from the predicted value.

[0075] Then, the state is updated. The predicted state is corrected by the Kalman gain and the updated state estimate is obtained by combining the measured data. It can be expressed by the following formula:

[0076]

[0077] in, is the updated state estimate, which combines the information of the predicted value and the measured value. k is the actual measurement data. is the predicted measurement value. is the measurement residual, which represents the difference between the prediction and the actual measurement.

[0078] Then, perform a covariance update. After the state is updated, the covariance matrix needs to be updated to reflect the new uncertainty. The updated covariance matrix is:

[0079] P k|k =(IK k H)P k|k-1

[0080] Where, the updated covariance represents the uncertainty of the current state estimate. I is the identity matrix, which ensures the stability of the covariance matrix update.

[0081] It can be understood that the prediction phase is responsible for predicting the state and covariance at the current moment based on the system model and the state estimate at the previous moment, with the purpose of providing initial values ​​for the update phase. The update phase corrects the prediction results through new measurement data, reduces errors, and updates the state and covariance to provide more accurate state estimates. As shown above, this application uses the construction of state transition equations and measurement equations to realize the construction of relevant formulas in the prediction process and update process.

[0082] Thus, the Kalman filter algorithm of this application gradually converges in a noisy environment by alternating prediction and update processes, ultimately obtaining a stable state estimate. Through the Kalman filter, high-frequency noise and angular velocity drift in the raw data are effectively suppressed, generating stable filtered trajectory data.

[0083] Step S103 , optimizing the initial trajectory data set after Kalman filtering by repeated data screening RDS algorithm to obtain an optimized trajectory data set, wherein the RDS algorithm uses the predicted joint axis direction vector to iteratively screen the sample data in the initial trajectory data set after Kalman filtering.

[0084] Specifically, the data quality is optimized based on the repeated data screening algorithm. Since the data collected by the IMU sensor may be inconsistent, this application uses the repeated data selection (RDS) algorithm to screen the sample data to enhance data reliability.

[0085] In order to more clearly illustrate the specific implementation process of the present application for screening the filtered initial trajectory data set, a trajectory data optimization method based on the RDS algorithm proposed in an embodiment of the present application is exemplified below.

[0086] Figure 3 This is a flow chart of a trajectory data optimization method based on the RDS algorithm proposed in an embodiment of the present application, such as Figure 3 As shown, the method includes the following steps:

[0087] Step S301 : constructing a residual function, wherein the residual function is used to calculate the difference between each sample data and a plurality of predicted joint axis direction vectors.

[0088] Specifically, in the screening strategy of the present embodiment, the residual function e(t) is constructed to measure the quality of the sample data points based on the deviation of the data. Specifically, the residual function e(t) determines the reliability of the data point by calculating the difference between each sample data and the joint axis direction vectors j1 and j2. This residual function plays an important role in the data screening process, helping to identify and eliminate noisy or erroneous data points.

[0089] The residual function e(t) is obtained by comparing the data q(t) at the current time point t with its predicted value (j1 and j2). The specific form is shown in the following formula:

[0090] e(t)=||q(t)×j1||-||q(t)×j2||

[0091] The residual function e(t) measures the deviation between the prediction and the actual measurement. When the value is large, it means that the sample data may contain noise or errors and should be eliminated.

[0092] Based on this, the main goal of the screening strategy in the embodiment of the present application is to delete data points with large errors through an iterative process. Each round of the screening process depends on the value of the residual function e(t).

[0093] Step S302: setting the initial sample capacity and the sample removal amount for each round of screening.

[0094] Specifically, during the screening process, the sample size is initialized, for example, setting the initial sample size N0 = 200, which means there are 200 sample data points at the beginning. Then, the number of samples with the largest residuals to be removed in each round of screening is determined. For example, in each iteration, based on the value of the residual function, n = 10 sample points with the largest residuals are selected and removed.

[0095] Step S303 , in each round of screening, the residual value of each current sample data is calculated by the residual function, the sample data with the largest residual value is removed, and the residual mean of each sample data remaining after the removal is calculated.

[0096] Step S304: When the residual mean corresponding to the current round of screening process is less than the residual mean corresponding to the previous round of screening process, or the number of remaining sample data is less than a preset threshold, the iterative screening process ends.

[0097] Specifically, during the iterative screening process, the residual mean me is updated. After each sample is removed, the residual mean me of the remaining data is updated. Convergence conditions are then determined. The screening process is stopped when the residual mean of the current round is less than or equal to the residual mean of the previous round, or when the number of remaining samples is less than a set threshold.

[0098] For example, suppose there are 200 sample data points, and the residual value of each sample data point is calculated using the residual function. In the first round of screening, the residual values ​​of all 200 data points are calculated and sorted from largest to smallest. The top 10 sample points with the largest residuals are selected and removed. This leaves 190 data points, and the residual mean me1 of these remaining 190 data points is then calculated.

[0099] In the second round of screening, the residual function is recalculated using the remaining 190 data points, and the top 10 largest residual samples are selected. These samples are then removed, and the residual mean at this point is updated to me2, and the next round of screening is continued, and so on.

[0100] Assuming that the newly calculated residual mean me3 in the third round of screening is smaller than the previous me2, or the number of remaining sample data is smaller than the set threshold (eg, 10), the screening process is stopped.

[0101] Therefore, this application gradually removes data points with large errors through the iterative screening process of the RDS algorithm, and the remaining sample data is more reliable, providing higher-quality data support for subsequent motion trajectory extraction.

[0102] Step S104 , calculating the joint axis direction by optimizing the trajectory data set, and constructing the dynamic motion trajectory of the dynamic life form according to the calculated joint axis direction and the angular velocity data after Kalman filtering.

[0103] Specifically, the joint axis orientations are estimated and the motion trajectory is extracted. First, the optimized trajectory dataset, obtained through Kalman filtering and the RDS algorithm in the above steps, is used. This optimized trajectory dataset is a high-precision, low-noise motion trajectory extracted from the raw IMU data. The data in the optimized trajectory dataset is further processed to calculate the joint axis orientations. The dynamic motion trajectory is constructed based on the joint axis orientations and the filtered angular velocities.

[0104] The joint axis direction j is a unit vector describing the orientation of the joint axis relative to the sensor coordinate system. Unlike the predicted vector, this step uses filtered trajectory data to calculate the joint axis direction. During the calculation, the joint axis direction can be constructed by obtaining angular velocity data after Kalman filtering.

[0105] In one embodiment of the present application, calculating the joint axis direction by optimizing the trajectory data set includes:

[0106] Obtain the roll angle data and pitch angle data from the optimized trajectory dataset, and calculate the joint axis direction based on the obtained angle data using the following formula:

[0107] j=[cos(φ i )cos(θ i ),sin(φ i )cos(θ i ),sin(θ i )]

[0108] Among them, φ i represents the roll angle at the i-th moment, θ i represents the pitch angle at the i-th moment, and j is a three-dimensional unit vector representing the direction of the joint axis. This calculation process calculates the spatial direction of the joint axis using the known angles (roll and pitch), ensuring an accurate description of the joint motion.

[0109] Furthermore, dynamic motion trajectories are constructed. After obtaining the joint axis directions, the angular velocity data is combined to construct dynamic motion trajectories. The joint axis directions and angular velocities are used to describe the motion path of the organism in a dynamic environment.

[0110] In one embodiment of the present application, constructing a dynamic motion trajectory of a dynamic life form includes: calculating the product of the joint axis direction and the corresponding angular velocity data, integrating the product, and obtaining the dynamic motion trajectory of the moving joint in three-dimensional space.

[0111] Specifically, during the construction of this embodiment, the trajectory evolution can be calculated based on the joint axis direction j and the angular velocity data after Kalman filtering. This process can be calculated using the following formula:

[0112] Motion trajectory = ∫(angular velocity × joint axis direction) dt

[0113] It is understood that the product of angular velocity and joint axis direction represents the actual motion change of the joint in space. By integrating this quantity, the dynamic trajectory of the joint in three-dimensional space can be obtained. For each key motion joint of a dynamic life form, the calculation method described above can be used, and the implementation principle is the same, so this application will not elaborate on this.

[0114] Therefore, the present application processes the original trajectory data through Kalman filtering and repeated data screening, which greatly reduces the influence of sensor noise and data errors, making the calculation of the joint axis direction and the trajectory construction more accurate and stable. Most of the related embodiments fail to fully process high-noise data, especially in complex dynamic environments, where the error in trajectory calculation is large. Through the above-mentioned processing, the present application effectively avoids the accumulation of noise and errors, ensures the accuracy and reliability of the motion trajectory, and can not only improve the estimation accuracy of the joint axis direction, but also effectively construct the motion trajectory.

[0115] Based on the above embodiment, after completing the extraction of the dynamic motion trajectory, the present application can also optimize and compress the trajectory data for storage, so as to perform applications such as robot control and motion analysis.

[0116] In one embodiment of the present application, after constructing the dynamic motion trajectory of a dynamic life form, it also includes: segmenting and storing the dynamic motion trajectory data according to changes in the motion state; compressing each segment of the dynamic motion trajectory, screening out key data points of the trajectory, and gradually controlling the exoskeleton robot according to the key data points of the trajectory of each segment of the dynamic motion trajectory.

[0117] Specifically, after acquiring motion data through the IMU sensor, this application uses Kalman filtering and data screening to obtain an accurate motion trajectory. To facilitate storage and subsequent control, this application stores the obtained trajectory data in segments. Each segment of trajectory data stores the joint angle, joint velocity, and related parameters for that segment of motion.

[0118] For example, the first segment of trajectory data stores the motion state of a living organism from rest to start, including joint angles and angular velocities. The second segment of trajectory data records the changes in joints, acceleration, or other kinematic information during the movement of the living organism.

[0119] Then, data compression is performed. The data of each dynamic motion trajectory is compressed and stored, retaining only key data points related to the joints. For example, key trajectory data points include joint angles, joint angular velocities, and other kinematic parameters. These data points can fully represent the joint motion of the trajectory.

[0120] By using the segmented and compressed trajectory data, the robot can be gradually controlled based on the joint data of each segment. Consider an exoskeleton-based robot whose motion trajectory is controlled based on data such as joint angles, velocities, and positions. During movement, the robot gradually adjusts the motion parameters of each segment based on the segmented trajectory data.

[0121] In order to more clearly illustrate the specific implementation process of the present application for gradually controlling the exoskeleton robot based on the key data points of the trajectory of each dynamic motion trajectory, a robot control method proposed in an embodiment of the present application is exemplified below.

[0122] Figure 4 This is a flow chart of a control method for an exoskeleton robot proposed in an embodiment of the present application, such as Figure 4 As shown, the method includes the following steps:

[0123] Step S401: Initialize the control system of the exoskeleton robot and load key data points of each segment of the dynamic motion trajectory.

[0124] Specifically, the robot control system is initialized. When the robot starts to move, the trajectory data of each segment obtained in the above embodiment is loaded in sequence, and the control system is initialized to set the target trajectory and control target of the robot for this task.

[0125] Step S402 , in accordance with the time sequence of each segment of the dynamic motion trajectory, and in turn according to the joint angle and joint angular velocity of each segment of the dynamic motion trajectory, controls the exoskeleton robot to perform corresponding operations.

[0126] For example, during step-by-step motion control, the robot's control system reads each segment of trajectory data and drives the robotic arm based on the joint angle and angular velocity data. Assuming the first segment of trajectory data covers the transition from rest to start-up, the control system sets the robot's initial motion based on this data. By reading the joint angles and angular velocities in this segment, the control system adjusts the robot's actuator output in real time, ensuring that the joints follow the set trajectory, matching the trajectory of a living organism.

[0127] Then, a smooth transition to the next trajectory segment occurs. When the first trajectory segment completes, the control system automatically switches to the second trajectory segment and continues control according to that data. If, during the second trajectory segment, the robot needs to adjust to a new angle or speed, the system adjusts the output parameters based on the new trajectory data to ensure a smooth transition. For example, if the second trajectory segment describes the robot's uniform motion after reaching a certain speed, the control system will adjust the robot's speed and angle in real time based on that data to ensure smooth motion.

[0128] Step S403: During the operation of the exoskeleton robot, feedback data from the exoskeleton robot is received in real time, and the feedback data is compared with the dynamic motion trajectory data, and the movement of the exoskeleton robot is adjusted according to the comparison result.

[0129] Specifically, real-time feedback and adjustments are performed during the operation of the exoskeleton robot. During the entire process, the robot continuously receives feedback data from different sensors and compares it with the trajectory data. The control system adjusts the action in real time based on the feedback data to ensure the accuracy of the robot's movement. For example, the final position of the robotic arm collected by the position sensor is obtained, and a comparison is made to determine whether the position the robotic arm actually reaches is different from the expected position. If the sensor detects a motion error (for example, an angular deviation or a speed deviation), the control system will promptly correct it through a compensation algorithm to ensure that the robot's movement conforms to the expected trajectory.

[0130] Based on the above embodiments, after completing the control of the exoskeleton robot, data optimization and subsequent analysis can also be performed. In one embodiment of the present application, after gradually controlling the exoskeleton robot, the process also includes: storing the actual execution data of the exoskeleton robot at different trajectory segments; evaluating the actual execution data of different trajectory segments, and optimizing the data processing strategy for the dynamic motion trajectory and the control strategy of the exoskeleton robot based on the evaluation results.

[0131] Specifically, after the robot completes its movement, its actual trajectory data is compressed and stored in a database, facilitating subsequent data analysis and optimization. During control model optimization, by evaluating the robot's performance at different trajectory segments, the system can optimize data processing and control strategies for subsequent trajectories, improving the robot's control accuracy and responsiveness.

[0132] For example, the robot's control strategy can be adjusted based on the discrepancies between the robot's actual execution and the expected trajectory in different trajectory segments. This includes adjusting the key trajectory data points that are prioritized when the robot performs different actions. For example, when the robot arm bends, the robot's control focuses on the joint angles in the key trajectory data points. Furthermore, the compression of each dynamic motion trajectory segment and the selection strategy for key trajectory data points can be adjusted based on the required key trajectory data points. For example, the compression rate of the trajectory data can be adjusted.

[0133] Thus, the present application uses segmented stored trajectory data to allow the robot to gradually control the motion of each of its joints. Each segment of trajectory data provides the corresponding joint angle, angular velocity, and other kinematic parameters. The control system accurately adjusts the motion of the robot joint based on this data, achieving high-precision dynamic control.

[0134] In summary, the dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening in the embodiment of the present application first dynamically corrects the motion model through Kalman filtering, effectively reducing the impact of sensor noise and integral error on the trajectory extraction results, and realizing dynamic filtering compensation. Then, the collected raw data is iteratively screened for multiple rounds through the repeated data screening algorithm, effectively eliminating high-error data points, which is conducive to enhancing the accuracy of trajectory estimation. Thus, this method combines the Kalman filtering algorithm with the repeated data screening algorithm for trajectory extraction, significantly improving the robustness and accuracy of dynamic motion trajectory extraction of living organisms. Moreover, this method can provide stable trajectory estimation results in different dynamic scenarios, adapt to the motion patterns of different living organisms, overcome the sensitivity of initial conditions to optimization results, enhance the robustness of dynamic motion trajectory extraction, and can be applied to the extraction of living organism motion trajectories in a variety of dynamic scenarios. As a result, this method improves the accuracy, robustness and reliability of dynamic motion trajectory extraction of living organisms, enriches the scenarios in which the motion trajectory extraction algorithm can be applied, improves the efficiency of motion trajectory extraction, and provides effective technical support for applications such as exoskeleton robots and rehabilitation training equipment.

[0135] In order to implement the above embodiment, the present application also proposes a dynamic motion trajectory extraction system based on Kalman filtering and repeated data screening. Figure 5 This is a structural diagram of a dynamic motion trajectory extraction system based on Kalman filtering and repeated data screening proposed in an embodiment of the present application, as shown in FIG. Figure 5 As shown, the system includes: a collection module 100, a filtering module 200, a screening module 300 and an extraction module 400.

[0136] Among them, the acquisition module 100 is used to install inertial measurement units IMU at multiple key motion joints of a dynamic life body, and collect the three-axis acceleration and three-axis angular velocity of the corresponding key motion joints during the movement process through the IMU to generate an initial trajectory data set.

[0137] The filtering module 200 is used to set a state space model based on the measurement data and Euler angles in the initial trajectory data set, and perform noise compensation on the initial trajectory data set through a Kalman filter based on the state space model.

[0138] The screening module 300 is used to optimize the initial trajectory data set after Kalman filtering by repeated data screening RDS algorithm to obtain an optimized trajectory data set, wherein the RDS algorithm uses the predicted joint axis direction vector to iteratively screen the sample data in the initial trajectory data set after Kalman filtering.

[0139] The extraction module 400 is used to calculate the joint axis direction by optimizing the trajectory data set, and construct the dynamic motion trajectory of the dynamic life body based on the calculated joint axis direction and the angular velocity data after Kalman filtering.

[0140] Optionally, in one embodiment of the present application, the filtering module 200 is specifically used to: construct a state vector based on the Euler angle and the angular velocity of the Euler angle, and construct a measurement vector based on the three-axis acceleration and three-axis angular velocity collected by the IMU; construct a state transfer equation based on the state vector, and construct a measurement equation based on the measurement vector; based on the state transfer equation and the measurement equation, iteratively perform the prediction process and the update process in the Kalman filter algorithm to suppress noise and angular velocity drift in the initial trajectory data set.

[0141] Optionally, in one embodiment of the present application, the screening module 300 is specifically used to: construct a residual function, wherein the residual function is used to calculate the difference between each sample data and multiple predicted joint axis direction vectors; set the initial sample capacity and the sample removal amount for each round of screening; in each round of screening, calculate the residual value of each current sample data through the residual function, remove the sample data with the largest residual value, and calculate the residual mean of each sample data remaining after removal; when the residual mean corresponding to the current screening process is less than the residual mean corresponding to the previous screening process, or the number of remaining sample data is less than a preset threshold, end the iterative screening process.

[0142] Optionally, in one embodiment of the present application, the extraction module 400 is specifically configured to obtain roll angle data and pitch angle data from the optimized trajectory data set, and calculate the joint axis direction based on the obtained angle data using the following formula:

[0143] j=[cos(φ i )cos(θ i ),sin(φ i )cos(θ i ),sin(θ i )]

[0144] Among them, φ i represents the roll angle at the i-th moment, θ i represents the pitch angle at the i-th moment, and j is a three-dimensional unit vector representing the direction of the joint axis; the product of the joint axis direction and the corresponding angular velocity data is calculated, and the product is integrated to obtain the dynamic motion trajectory of the moving joint in three-dimensional space.

[0145] Optionally, in one embodiment of the present application, the extraction module 400 is also used to: segmentally store the dynamic motion trajectory data according to changes in the motion state; compress each segment of the dynamic motion trajectory, filter out key data points of the trajectory, and gradually control the exoskeleton robot based on the key data points of the trajectory of each segment of the dynamic motion trajectory.

[0146] Optionally, in one embodiment of the present application, the extraction module 400 is also used to: initialize the control system of the exoskeleton robot and load the key data points of the trajectory of each segment of the dynamic motion trajectory; control the exoskeleton robot to perform corresponding operations according to the joint angle and joint angular velocity of each segment of the dynamic motion trajectory in the time sequence of each segment of the dynamic motion trajectory; receive feedback data of the exoskeleton robot in real time during the operation of the exoskeleton robot, compare the feedback data with the dynamic motion trajectory data, and adjust the action of the exoskeleton robot according to the comparison result.

[0147] Optionally, in one embodiment of the present application, the extraction module 400 is also used to: store the actual execution data of the exoskeleton robot in different trajectory segments; evaluate the actual execution data of different trajectory segments, and optimize the data processing strategy of the dynamic motion trajectory and the control strategy of the exoskeleton robot based on the evaluation results.

[0148] It should be noted that the aforementioned explanation of the embodiment of the dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening is also applicable to the system of this embodiment and will not be repeated here.

[0149] In summary, the dynamic motion trajectory extraction system based on Kalman filtering and repeated data screening in the embodiment of the present application improves the accuracy, robustness and reliability of the dynamic motion trajectory extraction of living organisms, enriches the applicable scenarios of the motion trajectory extraction algorithm, improves the efficiency of motion trajectory extraction, and provides effective technical support for applications such as exoskeleton robots and rehabilitation training equipment.

[0150] In order to implement the above-mentioned embodiments, the present application also proposes a non-temporary computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, it implements the dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening as described in any one of the above-mentioned first aspect embodiments.

[0151] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0152] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of the technical features being referred to. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of such features. Throughout the description of this application, "plurality" means at least two, for example, two, three, etc., unless otherwise specifically defined.

[0153] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing the steps of a custom logical function or process, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may be performed out of the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present application belong.

[0154] The logic and / or steps represented in the flowcharts or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing the logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (e.g., a computer-based system, a system including a processor, or other system that can fetch and execute instructions from an instruction execution system, apparatus, or device). For purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include the following: an electrical connection with one or more wires (electronic devices), a portable computer disk cartridge (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and programmable read-only memory (EPROM or flash memory), fiber optic devices, and a portable compact disc read-only memory (CDROM). Furthermore, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing it in another suitable manner if necessary, and then storing it in a computer memory.

[0155] It should be understood that various parts of the present application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used to implement: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application-specific integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0156] Those skilled in the art will understand that all or part of the steps in the method of the above embodiment can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

[0157] In addition, the functional units in the various embodiments of the present application may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into a module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0158] The storage medium mentioned above may be a read-only memory, a magnetic disk, or an optical disk, etc. Although the embodiments of the present application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present application. Persons skilled in the art may make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present application.

Claims

1. A dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening, characterized in that: The following steps are involved: An inertial measurement unit (IMU) is installed at each of the key motion joints of the dynamic life form, and the three-axis acceleration and three-axis angular velocity of the corresponding key motion joints during motion are collected by the IMU to generate an initial trajectory data set; A state-space model is set based on the measurement data and Euler angles in the initial trajectory dataset, and noise compensation is performed on the initial trajectory dataset through a Kalman filter based on the state-space model, wherein the setting of the state-space model based on the measurement data and Euler angles in the initial trajectory dataset includes: constructing a state vector based on the Euler angles and the angular velocity of the Euler angles, and constructing a measurement vector based on the three-axis acceleration and three-axis angular velocity collected by the IMU; the performing of noise compensation on the initial trajectory dataset through a Kalman filter based on the state-space model includes: constructing a state transfer equation based on the state vector, and constructing a measurement equation based on the measurement vector; and iteratively performing a prediction process and an update process in a Kalman filter algorithm based on the state transfer equation and the measurement equation to suppress noise and angular velocity drift in the initial trajectory dataset; The initial trajectory data set after Kalman filtering is optimized by a repeated data screening RDS algorithm to obtain an optimized trajectory data set, wherein the RDS algorithm uses the predicted joint axis direction vector to iteratively screen the sample data in the initial trajectory data set after Kalman filtering, and the optimization of the initial trajectory data set after Kalman filtering by the repeated data screening RDS algorithm includes: constructing a residual function, wherein the residual function is used to calculate the difference between each of the sample data and multiple predicted joint axis direction vectors; setting the initial sample capacity and the sample removal amount for each round of screening; in each round of screening, calculating the residual value of each current sample data by the residual function, removing the sample data with the largest residual value before the sample removal amount, and calculating the residual mean of each sample data remaining after removal; when the residual mean corresponding to the current screening process is less than the residual mean corresponding to the previous screening process, or the number of remaining sample data is less than a preset threshold, the iterative screening process is ended; The joint axis direction is calculated using the optimized trajectory data set, and the dynamic motion trajectory of the dynamic life form is constructed based on the calculated joint axis direction and the angular velocity data after Kalman filtering.

2. The method according to claim 1, characterized in that Calculating the joint axis direction using the optimized trajectory data set includes: Obtain roll angle data and pitch angle data in the optimized trajectory data set, and calculate the joint axis direction based on the obtained angle data using the following formula: in, Indicates the i The roll angle at the moment, Indicates the i The pitch angle at the moment, j is a three-dimensional unit vector representing the direction of the joint axis; The step of constructing the dynamic motion trajectory of the dynamic life form includes: The product of the joint axis direction and the corresponding angular velocity data is calculated, and the product is integrated to obtain the dynamic motion trajectory of the motion joint in three-dimensional space.

3. The method according to claim 1, characterized in that After constructing the dynamic motion trajectory of the dynamic life form, the method further includes: According to the change of the motion state, the dynamic motion trajectory data is stored in segments; Each dynamic motion trajectory is compressed, key data points of the trajectory are screened out, and the exoskeleton robot is gradually controlled based on the key data points of each dynamic motion trajectory.

4. The method according to claim 3, characterized in that The key trajectory data points include joint angles and joint angular velocities. The exoskeleton robot is gradually controlled according to the key trajectory data points of each dynamic motion trajectory, including: Initializing the control system of the exoskeleton robot and loading the key data points of the trajectory of each segment of the dynamic motion trajectory; According to the time sequence of the dynamic motion trajectories, the exoskeleton robot is controlled to perform corresponding operations according to the joint angle and the joint angular velocity of each dynamic motion trajectory; During the operation of the exoskeleton robot, feedback data of the exoskeleton robot is received in real time, and the feedback data is compared with the dynamic motion trajectory data, and the action of the exoskeleton robot is adjusted according to the comparison result.

5. The method according to claim 3, characterized in that After the exoskeleton robot is gradually controlled, the method further includes: Storing actual execution data of the exoskeleton robot at different trajectory segments; The actual execution data of the different trajectory segments are evaluated, and the data processing strategy of the dynamic motion trajectory and the control strategy of the exoskeleton robot are optimized according to the evaluation results.

6. A dynamic motion trajectory extraction system based on Kalman filtering and repeated data screening, characterized in that: include: An acquisition module is used to install inertial measurement units (IMUs) at multiple key motion joints of a dynamic life form, and to collect the three-axis acceleration and three-axis angular velocity of the corresponding key motion joints during motion through the IMUs to generate an initial trajectory data set; a filtering module, configured to set a state-space model based on the measurement data and Euler angles in the initial trajectory dataset, and to perform noise compensation on the initial trajectory dataset using a Kalman filter based on the state-space model, wherein the filtering module is specifically configured to: construct a state vector based on the Euler angles and the angular velocity of the Euler angles, and construct a measurement vector based on the three-axis acceleration and three-axis angular velocity collected by the IMU; construct a state transfer equation based on the state vector, and construct a measurement equation based on the measurement vector; Iteratively performing a prediction process and an update process in a Kalman filter algorithm based on the state transfer equation and the measurement equation to suppress noise and angular velocity drift in the initial trajectory data set; A screening module is used to optimize the initial trajectory data set after Kalman filtering by using a repeated data screening RDS algorithm to obtain an optimized trajectory data set, wherein the RDS algorithm uses the predicted joint axis direction vector to iteratively screen the sample data in the initial trajectory data set after Kalman filtering, and the screening module is specifically used to: construct a residual function, wherein the residual function is used to calculate the difference between each of the sample data and multiple predicted joint axis direction vectors; set the initial sample capacity and the sample removal amount for each round of screening; in each round of screening, calculate the residual value of each current sample data by using the residual function, remove the sample data with the largest residual value before the sample removal amount, and calculate the residual mean of each sample data remaining after removal; when the residual mean corresponding to the current screening process is less than the residual mean corresponding to the previous screening process, or the number of remaining sample data is less than a preset threshold, end the iterative screening process; The extraction module is used to calculate the joint axis direction through the optimized trajectory data set, and construct the dynamic motion trajectory of the dynamic life body according to the calculated joint axis direction and the angular velocity data after Kalman filtering.

7. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the dynamic motion trajectory extraction method based on Kalman filtering and repeated data screening according to any one of claims 1 to 5 is implemented.

Images