A method for tracking arm motion trajectory based on Tag-UWB

Abstract

The present invention discloses an arm motion trajectory tracking method based on Tag-UWB, which belongs to the field of arm posture tracking technology; the method is: using Tag-UWB technology to obtain arm joint position coordinate information; using least squares method to optimize the rough arm joint position coordinates; using EKF algorithm to dynamically adjust state estimation to achieve optimal estimation of system state; establishing BiLSTM arm trajectory tracking model, end-to-end learning and prediction of arm motion trajectory data, and prediction of future arm position and trajectory. The present invention converts the collected data into the spatial coordinates of each tag through the PDoA positioning model; effectively improves the accuracy and performance of the tracking system, provides support for the needs of high-precision positioning, and improves spatial scalability; the BiLSTM arm trajectory prediction model captures the changing characteristics in the arm trajectory; removes noise and accurately presents the actual movement path of the arm.

Description

Technical Field

[0001] The present invention belongs to the technical field of arm posture tracking, and in particular relates to an arm motion trajectory tracking method based on Tag-UWB. Background Art

[0002] The research and development deployment of key common technologies for the new generation of artificial intelligence requires algorithms as the core, data and hardware as the basis, and focuses on improving perception recognition, cognitive reasoning, motion execution and human-computer interaction capabilities. Among them, tracking the trajectory of human arm movements has always been a research hotspot in the field of human-computer interaction. Behavior operations through identifying arm movements are a generally recognized human-computer interaction method. Perceiving and identifying arm movement trajectories is crucial for monitoring user posture and body shape, and identifying user behavioral intentions, providing important support for complex applications.

[0003] To achieve motion tracking of human limbs, a common approach is to place ultra-wideband beacons on the user's limbs and torso in the environment of one or more receivers to obtain measurements.

[0004] Based on the user's existing smartphones and smart watches, a bidirectional long short-term memory network BiLSTM is designed to correct the original UWB data. The corrected UWB data and IMU data are ingested using RNN to output an arm posture sequence containing the 3D joint positions of the wrist, elbow, and shoulder, realizing a software-only arm posture tracking method. There is also a fusion of UWB sensors for measuring limb distance, IMU sensors for capturing the direction of part of the torso, and photodiodes for identifying finger flexion and extension signals. By creating a soft boundary buffer to leave space for non-signal actions or body signal changes, a supervised neural network is used to train a binary classifier to perform signal detection. Considering the geometric constraints of the human body on the node topology, especially for the Gaussian ranging error model and arm motion tracking problems, the maximum likelihood estimation rule is derived, and the unconstrained and constrained Cramer-Rao lower bounds are calculated.

[0005] In the research of UWB positioning algorithms, how to improve the system positioning accuracy has also been a hot topic for scholars in recent years.

[0006] Therefore, how to improve the positioning accuracy of the arm motion trajectory tracking system is the technical problem that the present invention aims to solve. Summary of the invention

[0007] The purpose of the present invention is to provide an arm motion trajectory tracking method based on Tag-UWB to solve the problems raised in the above background technology.

[0008] The object of the present invention is achieved by: a method for tracking arm motion trajectory based on Tag-UWB, characterized in that the method comprises the following steps:

[0009] Step S1: using Tag-UWB technology to obtain arm joint position coordinate information;

[0010] Step S2: Optimize the rough arm joint position coordinates using the least squares method;

[0011] Step S3: dynamically adjust the state estimation using the EKF algorithm to achieve the optimal estimation of the system state;

[0012] Step S4: Establish a BiLSTM arm trajectory tracking model to perform end-to-end learning and prediction of the arm motion trajectory data to achieve prediction of future arm position and trajectory.

[0013] Preferably, in step S1, the Tag-UWB technology is used to obtain the arm joint position coordinate information, and the specific operations are:

[0014] Step S1-1: Establish a UWB-PDoA base station, use the UWB-PDoA base station to obtain the received signal, measure the arrival phase difference, and obtain the path difference ;

[0015] The UWB-PDoA base station includes antenna A and antenna B. The distance between the antennas , is the wavelength of UWB signal;

[0016] The received signal is represented as:

[0017] ;

[0018] ;

[0019] in, Represents the signal emitted by the tag, is the phase difference, is the carrier frequency, Corresponding time delay, is an imaginary unit; is the signal received by antenna A, is the signal received by antenna B; from this, the phase difference and delay are obtained, and the path difference is calculated :

[0020] ;

[0021] ;

[0022] Step S1-2: Use the PDoA three-dimensional positioning principle to obtain the arm joint position coordinate information.

[0023] Preferably, in step S1-2, the PDoA three-dimensional positioning principle is used to obtain the arm joint position coordinate information, specifically:

[0024] The principle of PDoA three-dimensional positioning is that two PDoA base stations realize the three-dimensional spatial positioning of a single target, which is regarded as the intersection of two two-dimensional planes in space;

[0025] Let the coordinates of label C be , the coordinates of base station A are , the coordinates of base station B are , the projection of label C on the XOY plane is point D, and its projection on the x-axis and y-axis are point A and point B respectively;

[0026] Euclidean distance from tag C to base station A , the relative position of tag C to base station A is , ; Euclidean distance from label C to base station B , the relative position of tag C to base station B, that is, , ;

[0027] From the geometric relationship:

[0028] ;

[0029] This solves the label C Axis coordinates;

[0030] The coordinates of label C are:

[0031] ;

[0032] or:

[0033] ;

[0034] in, , is the coordinate of base station B relative to base station A, is the Euclidean distance between the tag and base station A; , They are the horizontal components of the tag vectors pointed to by base station A and base station B respectively;

[0035] ;

[0036] is the distance between the two antennas of base station A, Calculated by phase difference; by calculating and processing the measured PDoA data and distance data, the rough joint coordinate position is obtained.

[0037] Preferably, in step S2, the least square method is used to optimize the rough arm joint position coordinates, specifically:

[0038] The least squares method minimizes the sum of squared errors between actual observations and model predictions by solving for unknown variables.

[0039] make is the coordinate to be optimized and corrected, then:

[0040] ;

[0041] Arranged:

[0042] ;

[0043] in, , ;

[0044] Subtracting two adjacent equations gives:

[0045] ;

[0046] in, , Similarly;

[0047] make , , ,have to: ;

[0048] The error vector is denoted as ,Right now

[0049] ;

[0050] make For infinite time, we get The optimal solution of

[0051] ;

[0052] right Take the derivative:

[0053] ;

[0054] For a non-singular array, There is a unique solution, and the coordinates to be corrected and optimized are:

[0055] ;

[0056] in, is the transpose symbol for matrix operations.

[0057] Preferably, in step S3, the EKF algorithm is used to dynamically adjust the state estimation to achieve the optimal estimation of the system state, specifically:

[0058] After processing the data collected by Tag-UWB, the coordinates of a tag are obtained. , the coordinates As the system observable, the system model is as follows:

[0059] ;

[0060] in, is the Tag-UWB tag sampling frequency, Time location coordinates , and Approximately the first and second derivatives of the dynamic equation at the previous moment; for The moment position coordinate vector, for The first-order derivative of the dynamic equation at time ; the system state transfer equation is;

[0061] ;

[0062] in, is the state vector, ,Depend on Time position coordinate vector , the velocity vector ( Stitched together, Respectively represent the x, y, and z components of the velocity vector;

[0063] , , , is the control input vector; , is Gaussian white noise; the measurement function is:

[0064] ;

[0065] For base station The coordinates of the measurement function are:

[0066] ;

[0067] in, Indicates base station With label in Euclidean distance in the axis direction, , , Respectively represent the estimated position coordinates at the kth moment , , The measurement equation of the system is:

[0068] ;

[0069] in, , calculate the Kalman gain:

[0070] ;

[0071] in, is the measurement noise The covariance matrix of , updates the system state vector and error covariance:

[0072] ;

[0073] ;

[0074] in, is the identity matrix, For The estimated value of the state vector at time t; For The Kalman gain at time t.

[0075] Preferably, the BiLSTM arm trajectory tracking model is a variant of the LSTM model in structure, and a reverse transmission pathway is added to the LSTM model;

[0076] The BiLSTM arm trajectory tracking model includes a forward LSTM layer and a reverse LSTM layer, and each of the forward LSTM layer and the reverse LSTM layer has 8 hidden units;

[0077] A dropout layer is added between the forward LSTM layer and the reverse LSTM layer.

[0078] Preferably, in step S4, a BiLSTM arm trajectory tracking model is established to perform end-to-end learning and prediction on the arm motion trajectory data, and the specific operations are as follows:

[0079] The training process of the BiLSTM arm trajectory tracking model includes forward propagation, loss calculation, back propagation, and parameter update. The forward LSTM propagation of the forward LSTM layer is to make each LSTM unit update its hidden state and memory unit through its input gate, forget gate, and output gate;

[0080] The reverse LSTM propagation of the reverse LSTM layer is to pass the input sequence backward to the reverse LSTM unit, and gradually process the data from the end to the beginning of the sequence. The concatenation stage concatenates the forward and reverse hidden states into a larger vector;

[0081] Optimize model performance by adjusting batch size and learning rate hyperparameters, use Adam optimizer to adaptively adjust learning rate, and use different training rounds for different data sets;

[0082] Use the data loader to load data into the model in batches for training. After training, use the test set to evaluate the model and obtain the performance of the model on unseen data as the final performance indicator of the model.

[0083] Ultimately, the result of the forward propagation is the model's prediction of the input data.

[0084] Compared with the prior art, the present invention has the following improvements and advantages:

[0085] 1. The positioning model based on the PDoA method converts the collected data into the spatial coordinates of each tag by deploying one or more UWB tags and base stations at the elbow and wrist joints of the arm; it effectively improves the accuracy and performance of the tracking system, provides support for the needs of high-precision positioning, and also has great scalability.

[0086] 2. The method of the present invention optimizes the coordinates of the wrist joint through the least squares method, uses the extended Kalman filter algorithm to filter the label trajectory, provides the optimal state estimation, and captures the complex patterns and changing characteristics in the arm trajectory based on the BiLSTM arm trajectory prediction model; removes the existing noise and more accurately presents the actual movement path of the arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 The figure is an overall flow chart of the method of the present invention.

[0088] Figure 2 Schematic diagram of the two-dimensional positioning principle of PdoA.

[0089] Figure 3 This is the schematic diagram of PDoA three-dimensional positioning principle.

[0090] Figure 4 This is the basic neuron structure diagram of LSTM.

[0091] Figure 5 This is the structural diagram of the BiLSTM arm trajectory tracking model.

[0092] Figure 6 This is the module diagram of the BiLSTM arm trajectory tracking model.

[0093] Figure 7 This is the error result diagram of the elbow joint test set.

[0094] Figure 8 This is the result graph of the distance between two labels for continuous unspecified actions.

[0095] Fig. 9 This is a comparison chart of the prediction errors between the specified action and the random action. DETAILED DESCRIPTION

[0096] The present invention is further summarized below with reference to the accompanying drawings.

[0097] like Figure 1 As shown, a method for tracking arm motion trajectory based on Tag-UWB includes the following steps:

[0098] Step S1: using Tag-UWB technology to obtain arm joint position coordinate information;

[0099] Step S1-1: Establish a UWB-PDoA base station, use the UWB-PDoA base station to obtain the received signal, measure the arrival phase difference, and obtain the path difference ;

[0100] The UWB-PDoA base station includes antenna A and antenna B. The distance between the antennas , is the wavelength of the UWB signal, and the arrival phase difference can be measured. The path difference is determined by the phase difference between the earliest signal path reaching antenna A and antenna B. Taking antenna A as the reference, the received signal can be expressed as:

[0101] ;

[0102] ;

[0103] in, Represents the signal emitted by the tag, is the phase difference, is the carrier frequency, Corresponding time delay, is an imaginary unit; is the signal received by antenna A, is the signal received by antenna B; from this, the phase difference and delay are obtained, and the path difference is calculated :

[0104] ;

[0105] ;

[0106] The PDoA-based two-dimensional positioning form is simplified as follows Figure 2 Model shown:

[0107] Let the coordinates of label C be , from the geometric relationship,

[0108] ;

[0109] Simplifying, we can get

[0110] ;

[0111] Considering that the distance between the two antennas in the UWB-PDoA base station module is smaller than the distance between the tag and the base station, can be ignored. Therefore, it can be simplified to:

[0112] ;

[0113] In general, the solved label C coordinates It is the relative position of a reference antenna relative to the base station. It can be obtained from the phase difference measured by the PDoA base station. is the distance between the two antennas, which can be measured directly. It is the Euclidean distance from the tag to the reference antenna, measured by the ToF ranging method integrated in the PDoA base station.

[0114] In summary, the two-dimensional PDOA positioning on the plane can obtain the coordinates of tag C with the base station as the origin and the Euclidean distance from the tag to the base station.

[0115] Step S1-2: using the PDoA three-dimensional positioning principle to obtain arm joint position coordinate information;

[0116] Analysis of the PDoA principle shows that using two PDoA base stations can achieve single-target three-dimensional spatial positioning, and redundant base stations can be used as an aid to correct errors and improve accuracy. Figure 3 As shown in Figure 1, the three-dimensional positioning based on the PDoA method can be regarded as the intersection of two two-dimensional planes in space:

[0117] Let the coordinates of label C be , the coordinates of base station A are , the coordinates of base station B are , the projection of label C on the XOY plane is point D, and its projection on the x-axis and y-axis are point A and point B respectively.

[0118] Planar AEC is a set of two-dimensional PDoA positioning measured by base station A. After two-dimensional PDoA positioning calculation, the data obtained is the Euclidean distance from tag C to base station A. , the relative position of tag C to base station A (positive along the positive direction of the axis, negative along the negative direction), that is, , Similarly, the plane BCF is a set of two-dimensional PDoA positioning measured by base station B. The data that can be obtained is the Euclidean distance from tag C to base station B. , the relative position of tag C to base station B, that is, , .

[0119] From the geometric relationship,

[0120] ;

[0121] This solves the label C Axis coordinates;

[0122] The coordinates of label C are:

[0123] ;

[0124] or:

[0125] ;

[0126] in, , is the coordinate of base station B relative to base station A, is the Euclidean distance between the tag and base station A; , That is, the horizontal component of the vectors pointing from the two base stations to the tag;

[0127] ;

[0128] is the distance between the two antennas of base station A, Calculated by phase difference; by calculating and processing the measured PDoA data and distance data, the rough joint coordinate position is obtained.

[0129] Step S2: Use the least squares method to optimize the rough arm joint position coordinates, specifically: based on the "label-label" tuple feature, that is, the user's forearm length is known and fixed. Combined with the known user's forearm length, the least squares method can be used to optimize the coordinate position. The benefits of this processing include convenient scaling according to the user's arm span, so as to better adapt to the physical characteristics of different users. This personalized processing method can improve the applicability and flexibility of the positioning system, making it more practical and scalable in future practical applications.

[0130] make is the coordinate to be optimized and corrected, then:

[0131] ;

[0132] Arranged:

[0133] ;

[0134] in, , ;

[0135] Subtracting two adjacent equations gives:

[0136] ;

[0137] in, , Similarly;

[0138] make , , ,have to: ;

[0139] The error vector is denoted as ,Right now

[0140] ;

[0141] make For infinite time, we get The optimal solution of

[0142] ;

[0143] right Take the derivative:

[0144] ;

[0145] For a non-singular array, There is a unique solution, and the coordinates to be corrected and optimized are:

[0146] ;

[0147] in, is the transpose symbol for matrix operations.

[0148] The least squares method is a mathematical optimization method. Its basic principle is to find the best matching result of data by minimizing the sum of squares of errors. This method aims to minimize the sum of squares of errors between actual observations and model predictions by solving unknown variables. The least squares method is simple to operate and is often used for problems such as curve fitting.

[0149] Step S3: Use the EKF algorithm to dynamically adjust the state estimation to achieve the optimal estimation of the system state, specifically:

[0150] Kalman Filter is an efficient method for estimating system states. It estimates the system state recursively by fusing system model predictions with sensor measurement data. This method is based on two assumptions: first, the system and observation models are linear; second, the interference noise follows a Gaussian distribution. When these prerequisites are met, the Kalman filter can achieve optimal state estimation.

[0151] ;

[0152] in, is the Tag-UWB tag sampling frequency, Time location coordinates , and Approximately the first and second derivatives of the dynamic equation at the previous moment; for The moment position coordinate vector, for The first-order derivative of the dynamic equation at the moment; the system state transfer equation is:

[0153] ;

[0154] in, is the state vector, ,Depend on Time position coordinate vector , the velocity vector ( Stitched together, Respectively represent the x, y, and z components of the velocity vector;

[0155] , , , is the control input vector; , is Gaussian white noise; the measurement function is:

[0156] ;

[0157] For base station The coordinates of the measurement function are:

[0158] ;

[0159] in, Indicates base station With label in Euclidean distance in the axis direction, , , Respectively represent the estimated position coordinates at the kth moment , , The measurement equation of the system is:

[0160] ;

[0161] in, , calculate the Kalman gain:

[0162] ;

[0163] in, is the measurement noise The covariance matrix of , updates the system state vector and error covariance:

[0164] ;

[0165] ;

[0166] in, is the identity matrix, For The estimated value of the state vector at time t; For The Kalman gain at time t.

[0167] Step S4: Establish a BiLSTM arm trajectory tracking model to perform end-to-end learning and prediction of the arm motion trajectory data to achieve prediction of future arm position and trajectory;

[0168] like Figure 5 , 6 As shown, the BiLSTM arm trajectory tracking model is a variant of the LSTM model in structure, adding a reverse transmission pathway to the LSTM model;

[0169] The BiLSTM arm trajectory tracking model includes a forward LSTM layer and a reverse LSTM layer, each of which has 8 hidden units;

[0170] A dropout layer is added between the forward LSTM layer and the reverse LSTM layer.

[0171] The training process of the BiLSTM arm trajectory tracking model includes forward propagation, loss calculation, back propagation, and parameter update. The forward LSTM propagation of the forward LSTM layer is to make each LSTM unit update its hidden state and memory unit through its input gate, forget gate, output gate, etc.

[0172] The reverse LSTM propagation of the reverse LSTM layer is to pass the input sequence backward to the reverse LSTM unit, and process the data step by step from the end of the sequence to the beginning. The concatenation stage concatenates the forward and reverse hidden states into a larger vector;

[0173] Optimize model performance by adjusting batch size and learning rate hyperparameters, use Adam optimizer to adaptively adjust learning rate, and use different training rounds for different data sets;

[0174] Use the data loader to load data into the model in batches for training. After training, use the test set to evaluate the model and obtain the performance of the model on unseen data as the final performance indicator of the model.

[0175] Ultimately, the result of the forward propagation is the model's prediction of the input data.

[0176] LSTM networks avoid the long-term dependency problem of typical RNNs. They improve the training and reasoning performance of complex sequences by looking back at past states; all recurrent neural networks have the form of a chain of repeated modules of neural networks, and LSTM also has this chain structure, but the repeated modules have a different structure. Figure 4 As shown in Figure 1, an LSTM unit and its components are shown, where f, i, o, and g are the forget gate, input gate, output gate, and candidate unit, respectively.

[0177] The component formula at time step t is:

[0178] ;

[0179] in, and are the neuron state and the hidden state, respectively. , and denote input weight, loop weight and bias respectively, is the input sequence.

[0180] In order to verify the feasibility and effect of the method of the present invention, the experimental results of high-precision arm trajectory tracking in this paper are presented through the arm trajectory tracking effect and action recognition accuracy:

[0181] The experimental setup includes a Windows 10 operating system with Python 3.8 installed, a Tag-UWB positioning system kit, including several PDoA base stations and tags.

[0182] Core hardware equipment - HR-RTLS1-PDOA wireless radio frequency tag (a) and base station (b). Each base station and tag integrates Decawave's DW3000 series UWB chip and uses STM32F103CBT6 as the core microcontroller unit, supporting time-of-flight distance measurement and arrival angle positioning functions to collect UWB data and realize data transmission between serial communication interfaces.

[0183] The UWB signal transmitted by the PDoA base station has a bandwidth of 500MHz and a frequency range of 6250 to 8250MHz. In this experiment, a frequency of 6250MHz was selected. The distance between the two antennas is about 0.02 meters, and the maximum effective range of the system is 40 meters. The data acquisition part mainly uses Python's serial package and threading package to create multiple threads to realize the synchronous reading of multiple serial port data; data processing, coordinate correction, trajectory filtering and other related work are completed in PyCharm 2023.2, and Python 3.8 and Pytorch 1.11.0 are used to build a neural network to complete the prediction and recognition of the arm trajectory.

[0184] In this experiment, the user performs arm movements while wearing the Tag-UWB tag by default, and all raw data is collected continuously. The collected data is mainly divided into two situations: the experimenter's tracked arm performs continuous specified movements and random movements. The ratio of the training set to the test set is 8:2. The whole system is trained end-to-end using the Adam optimizer using the mean square error (MSE) loss, and the learning rate is The subject's tracked arm performs long-term continuous movements with specified actions, and 4 sets of 1200-frame joint position timing data are used as training samples.

[0185] like Figure 7As shown in the figure, for the four sets of elbow joint position time series data, in the test set of 240 frames, the predicted results output at each time step are compared with the actual values. The corresponding median distance errors are 5.42cm, 5.97cm, 5.31cm, and 5.72cm, respectively. The maximum errors are 17.61cm, 16.50cm, 18.56cm, and 17.37cm, respectively. The minimum errors are 1.40cm, 1.52cm, 1.16cm, and 2.38cm, respectively. It can be seen that there are many outliers in the data, which are far away from the central trend of the data. These significant differences may be due to the fact that some data are affected by the non-line-of-sight environment, resulting in large data offsets.

[0186] like Figure 8 As shown in the figure, direct measurement shows that the distance between the subject's elbow and wrist is about 23.5 cm. The average error of the label spacing after trajectory correction is reduced by 69.91%. On this basis, the average error of the label spacing after trajectory filtering is reduced by 5.01%. Therefore, it can be seen that although the algorithm used in this study still deviates from the true value, the reduction in the average error of the label spacing proves that the algorithm has improved the accuracy and stability of the trajectory data to a certain extent.

[0187] When performing posture estimation, there are two situations: one is that the experimenter performs a series of continuous specified actions according to a predetermined pattern; the other is that the experimenter performs random, unspecified actions. These two situations need to be calculated separately, that is, in the test set, the predicted results output at each time step are compared with the actual value;

[0188] like Fig. 9 As shown in the figure, the average error of the prediction results for the elbow joint is 8.31cm, while that for the wrist joint is 10.85cm; in the case of designated actions, the average error is 7.32cm, while the average error for continuous random actions is 11.84cm. This shows that the prediction accuracy of different joints may vary. In most cases, the range of motion of the elbow joint is smaller than that of the wrist joint. This observation is consistent with the physiological characteristics and kinematic constraints of the human body. Therefore, it can be shown that the long-term dependencies in the time series learned by the neural network in this paper can capture the complex patterns and changing characteristics in the arm trajectory; in addition, the prediction error obtained for designated actions under control conditions is smaller than that of continuous random actions. This may be attributed to the fact that when performing specific actions, the participants' actions are more standardized and predictable, while continuous random actions introduce more variables and uncertainties, making tracking and prediction more difficult. Nevertheless, even in the case of continuous random actions, the model still shows relatively reasonable prediction ability, which shows that the neural network adopted has certain robustness in dealing with the variable arm movements in real life.

[0189] The above description is only an embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent substitution, improvement, etc. made within the spirit and principle of the present invention should be included in the scope of the claims of the present invention.

Claims

1. A method for tracking arm motion trajectory based on Tag-UWB, characterized in that: The method comprises the following steps: Step S1: using Tag-UWB technology to obtain arm joint position coordinate information; Step S1-1: Establish a UWB-PDoA base station, use the UWB-PDoA base station to obtain the received signal, measure the arrival phase difference, and obtain the path difference ; The UWB-PDoA base station includes antenna A and antenna B. The distance between the antennas , is the wavelength of UWB signal; The received signal is represented as: ； ； in, Represents the signal emitted by the tag, is the phase difference, is the carrier frequency, Corresponding time delay, is an imaginary unit; is the signal received by antenna A, is the signal received by antenna B; from this, the phase difference and delay are obtained, and the path difference is calculated : ； ； Step S1-2: using the PDoA three-dimensional positioning principle to obtain arm joint position coordinate information; The principle of PDoA three-dimensional positioning is that two PDoA base stations realize the three-dimensional spatial positioning of a single target, which is regarded as the intersection of two two-dimensional planes in space; Let the coordinates of label C be , the coordinates of base station A are , the coordinates of base station B are , the projection of label C on the XOY plane is point D, and its projection on the x-axis and y-axis are point A and point B respectively; Euclidean distance from tag C to base station A , the relative position of tag C to base station A is , ; Euclidean distance from label C to base station B , the relative position of tag C to base station B, that is , ; From the geometric relationship: ； This solves the label C Axis coordinates; The coordinates of label C are: ； or: ； in, , is the coordinate of base station B relative to base station A, is the Euclidean distance between the tag and base station A; , They are the horizontal components of the tag vectors pointed to by base station A and base station B respectively; ； is the distance between the two antennas of base station A, Calculated by phase difference; by calculating and processing the measured PDoA data and distance data, the rough joint coordinate position is obtained; Step S2: Optimize the rough arm joint position coordinates using the least squares method; Step S3: dynamically adjust the state estimation using the EKF algorithm to achieve the optimal estimation of the system state; After processing the data collected by Tag-UWB, the coordinates of a tag are obtained. , the coordinates As the system observable, the system model is as follows: ； in, is the Tag-UWB tag sampling frequency, Time location coordinates , and Approximately the first and second derivatives of the dynamic equation at the previous moment; for The moment position coordinate vector, for The first-order derivative of the dynamic equation at time ; the system state transfer equation is; ； in, is the state vector, ,Depend on Time position coordinate vector , the velocity vector ( Stitched together, Respectively represent the x, y, and z components of the velocity vector; , , , is the control input vector; , is Gaussian white noise; the measurement function is: ； For base station The coordinates of the measurement function are: ； in, Indicates base station With label in Euclidean distance in the axis direction, , , Respectively represent the estimated position coordinates at the kth moment , , The measurement equation of the system is: ； in, , calculate the Kalman gain: ； in, is the measurement noise The covariance matrix of , updates the system state vector and error covariance: ； ； in, is the identity matrix, For The estimated value of the state vector at time t; For Kalman gain at time t; Step S4: Establish a BiLSTM arm trajectory tracking model to perform end-to-end learning and prediction of the arm motion trajectory data to achieve prediction of future arm position and trajectory.

2. The arm motion trajectory tracking method based on Tag-UWB according to claim 1, characterized in that: In step S2, the least square method is used to optimize the rough arm joint position coordinates, specifically: The least squares method minimizes the sum of squared errors between actual observations and model predictions by solving for unknown variables. make is the coordinate to be optimized and corrected, then: ； Arranged: ； in, , ; Subtracting two adjacent equations gives: ； in, , Similarly; make , , ,have to: ; The error vector is denoted as ,Right now ； make For infinite time, we get The optimal solution of ； right To perform the derivation: ； For a non-singular array, There is a unique solution, and the coordinates to be corrected and optimized are: ； in, is the transpose symbol for matrix operations.

3. The arm motion trajectory tracking method based on Tag-UWB according to claim 1, characterized in that: The BiLSTM arm trajectory tracking model is a variant of the LSTM model in structure, adding a reverse transmission pathway to the LSTM model; The BiLSTM arm trajectory tracking model includes a forward LSTM layer and a reverse LSTM layer, and each of the forward LSTM layer and the reverse LSTM layer has 8 hidden units; A dropout layer is added between the forward LSTM layer and the reverse LSTM layer.

4. The arm motion trajectory tracking method based on Tag-UWB according to claim 3 is characterized in that: In step S4, a BiLSTM arm trajectory tracking model is established to perform end-to-end learning and prediction on the arm motion trajectory data. The specific operations are as follows: The training process of the BiLSTM arm trajectory tracking model includes forward propagation, loss calculation, back propagation, and parameter update. The forward LSTM propagation of the forward LSTM layer is to make each LSTM unit update its hidden state and memory unit through its input gate, forget gate, and output gate; The reverse LSTM propagation of the reverse LSTM layer is to pass the input sequence backward to the reverse LSTM unit, and gradually process the data from the end to the beginning of the sequence. The concatenation stage concatenates the forward and reverse hidden states into a larger vector; Optimize model performance by adjusting batch size and learning rate hyperparameters, use Adam optimizer to adaptively adjust learning rate, and use different training rounds for different data sets; Use the data loader to load data into the model in batches for training. After training, use the test set to evaluate the model and obtain the performance of the model on unseen data as the final performance indicator of the model. Ultimately, the result of the forward propagation is the model's prediction of the input data.

Images