High-precision UWB positioning method for TDOA scene

Abstract

The invention provides a high-precision UWB (ultra wide band) positioning method for a TDOA (time difference of arrival) scene, and relates to the technical field of UWB positioning. A UKF-FNN-Chang-RIC algorithm aiming at the TDOA scene is provided; firstly, unscented Kalman filtering is carried out on an obtained distance difference vector, and then a feedforward neural network (FNN) is introduced to correct the distance difference vector filtered by the UKF; converting the distance difference measurement value into a distance measurement value by using a Chan algorithm, and adjusting the distance measurement value through a gradient descent method; and finally, resolving through a least square method to obtain the position of the target.

Description

Technical Field

[0001] The present invention relates to the field of ultra-wideband positioning technology, and in particular to a high-precision UWB positioning method for TDOA scenarios. Background Art

[0002] With the rapid development and widespread application of ultra-wideband (UWB) technology in the field of precise positioning, positioning systems based on UWB technology have shown great potential and advantages in various scenarios. UWB technology has attracted extensive attention and research in many fields such as indoor positioning, Internet of Things, autonomous driving, drone navigation, and smart warehousing due to its high bandwidth, low power consumption, and high anti-interference ability. However, despite the many advantages of UWB technology, its positioning accuracy and performance are still affected by many factors, such as the multipath effect of signal propagation, environmental noise, obstacle occlusion, and signal interference. Therefore, further improving the accuracy and reliability of UWB positioning and developing high-precision positioning methods that adapt to complex environments have become the hot spots and difficulties of current research.

[0003] At present, the research on UWB technology mainly includes NLOS identification, optimal distribution of base stations, precision improvement algorithms based on TDOA, TOA, AOA scenarios, and research on the fusion system of UWB and other positioning technologies. The present invention conducts in-depth research on the UWB solution model based on the TDOA scenario, and uses the unscented Kalman filter UFK and the feedforward neural network FNN transformation to make the data meet the preconditions of its application, and proposes the UKF-FNN-Chan-RIC algorithm for the TDOA scenario in combination with the Chan algorithm and the RIC algorithm to improve the accuracy and robustness of the UWB system, and finally obtains the positioning position coordinates of the UWB system through the least squares solution. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides a high-precision UWB positioning method for TDOA scenarios to solve the positioning accuracy and robustness problems faced by current UWB positioning systems in complex environments.

[0005] A high-precision UWB positioning method for TDOA scenarios includes the following steps:

[0006] Step 1: Obtain the distance difference vector Δd between each UWB node;

[0007] Multiple base stations are arranged in the three-dimensional positioning area. The UWB tags transmit signals, and the base stations receive these signals and record the reception time. Each base station independently records the timestamp of the signal it receives. N UWB receiving nodes are set in the three-dimensional positioning area. The UWB receiving nodes record the time when the same signal arrives, calculate the distance difference based on the time difference, and obtain the distance difference vector.

[0008] Assume that the time when the i-th receiving node receives the signal is t i , the time when the jth receiving node receives the signal is t j , then the time difference Δt ij for:

[0009] Δt ij =t j -t i

[0010] The corresponding distance difference is:

[0011] Δd ij =c·Δt ij

[0012] Where c is the speed of light and the distance difference vector is:

[0013] Δd=[Δd 12 ,Δd 13 ,...,Δd 1N ,Δd 23 ,Δd 24 ,...,Δd 2N ,...,Δd (N-1)N ] T

[0014] Where, Δd ij Represents the distance difference between the i-th receiving node and the j-th receiving node; the distance difference vector includes the distance differences between all node pairs, with a total of N(N-1) / 2 elements.

[0015] Step 2: Propose the UKF-FNN-Chan-RIC algorithm for TDOA scenarios;

[0016] Step 2.1: Perform unscented Kalman filtering on the obtained distance difference vector;

[0017] In the unscented Kalman filter UKF, the state variable x of the target is k represents the position of the target in three-dimensional space at time k, where X k ,Y k ,Z k Respectively represent the coordinates of the target in three-dimensional space, namely:

[0018] x k =[X k ,Y k ,Z k ] T

[0019] UKF estimates the target position through the state transfer equation and observation equation;

[0020] The state transfer equation is:

[0021] x k+1 =f(x k )+w k

[0022] Among them, x k+1 is the state vector of the target at time k+1, f(x k ) is the state transfer function, w k is the process noise; the observation equation is:

[0023] Z k =h(x k )+V k

[0024] Among them, Z k is the observation vector obtained at time k, h(x k ) is the observation model, and the target state variable x k Replaced with the predicted distance difference vector, V k is the observation noise;

[0025] The observation model h(x k ) Calculate the target state variable x k The distances between the receiving nodes are calculated and the differences between these distances are used to form the predicted distance difference vector h(x k );

[0026]

[0027] Where X i , Y i , Z i is the known three-dimensional coordinate of the i-th receiving node, that is, the fixed position of these nodes in space. This formula calculates the Euclidean distance from the target position to each base station.

[0028] UKF processes nonlinear systems through unscented transformation: First, the sigma point is calculated:

[0029]

[0030] Among them, χ k is the set of sigma points, P k is the state covariance matrix, L is the dimension of the state variables, and λ is the adjustment parameter, where λ = α 2 (L+κ)-L, α and κ are UKF parameters;

[0031] The sigma point is predicted by the state transfer equation, and the set of predicted sigma points is χ k+1|k :

[0032] χk+1|k =f(χ k )

[0033] Calculate the predicted state mean and covariance P k+1 / k They are:

[0034]

[0035] Among them, W i m and W i c is the weight of the sigma point, It is the result after the i-th sigma point passes through the state transfer function, and Q is the covariance matrix of the process noise.

[0036] Predicted observation value Z k+1 for:

[0037] Z k+1 =h(χ k+1 |k)

[0038] Calculate the predicted observed mean and covariance P zz :

[0039]

[0040] in, is the result after the i-th sigma point passes through the observation model, and R is the covariance matrix of the observation noise.

[0041] Calculate the covariance matrix P between the state and the observation xz :

[0042]

[0043] The Kalman gain is K:

[0044]

[0045] Update the estimate of the mean of the observations for:

[0046]

[0047] Among them, h(z k+1 ) is the observation model for the predicted observations.

[0048] The filtered distance difference vector is the updated mean of the observations:

[0049]

[0050] Step 2.2: Introduce the feedforward neural network FNN to correct the distance difference vector after UKF filtering;

[0051] The structure of the feedforward neural network FNN consists of an input layer, a hidden layer and an output layer; the input of the input layer is the filtered distance difference vector Δd filtered The hidden layer processes the output of the previous layer through a nonlinear activation function, and the output layer outputs the corrected distance difference vector Δd corrected ; Assume that the dimension of the distance difference vector is p, then the number of neurons in the input layer and the output layer is p;

[0052] The FNN includes two hidden layers, specifically a first hidden layer and a second hidden layer; the first hidden layer has h1 neurons, and the second hidden layer has h2 neurons; for the first hidden layer, the output of the jth neuron for:

[0053]

[0054] in, represents the weight between the i-th element of the input vector and the j-th neuron in the first hidden layer, which is the weight parameter of the neuron; Δd filtered,i Denotes Δd filtered The i-th element of The bias term of the jth neuron in , σ is the activation function;

[0055] For the second hidden layer, the output of the kth neuron for:

[0056]

[0057] in, represents the weight connecting the jth neuron in the first hidden layer to the kth neuron in the second hidden layer; b k (2) is the bias vector of the kth neuron in the second hidden layer;

[0058] The output of the lth neuron in the output layer is:

[0059]

[0060] Where, Δd filtered,l represents the lth component of the corrected distance difference vector, represents the weight connecting the lth neuron in the second hidden layer to the kth neuron in the output layer, is the bias of the lth neuron in the output layer.

[0061] Through FNN training, minimize the corrected distance difference vector Δd corrected The difference vector Δd from the actual distance true The error between . Use mean square error MSE as the loss function:

[0062]

[0063] Among them, C is the loss function value, which represents the corrected distance difference vector Δd corrected And the distance difference vector Δd after UKF filtering filtered The error between correctrd,l represents the lth component of the corrected distance difference vector, Δd filtered,l represents the lth component of the distance difference vector after UKF filtering;

[0064] The weights and biases of the network are adjusted through the back-propagation algorithm to optimize the loss function. The trained FNN outputs the corrected distance difference vector Δd corrected ;

[0065] Step 2.3: Use Chan algorithm to convert the distance difference measurement value into the distance measurement value;

[0066] The distance difference vector Δd needs to be corrected Convert to an absolute distance vector. First, from the distance difference vector Δd corrected Extract the partial vector ΔD:

[0067] ΔD=[Δd 21 ,Δd 31 ,...,Δd N1 ]

[0068] The equation is constructed using the distance difference between base stations:

[0069] Δd i1 =d i -d1

[0070] Δd i1 Represents the distance d from base station i to the target node i The distance difference between the reference base station and the target node d1, d i is the actual distance from base station i to the target node, d1 is the actual distance from the reference base station to the target node;

[0071] After squaring, we get:

[0072]

[0073] Taking the reference base station as the origin, the distance of each base station relative to the reference base station is obtained:

[0074]

[0075] where p i represents the position vector of base station i, p1 represents the position vector of the reference base station, ||p i -p1|| represents the Euclidean distance between base station i and the reference base station, i = 2, 3, ..., N;

[0076] Therefore, for each base station, the distance vector d calculated using the distance difference equation is Chan It is expressed as:

[0077] d Chan =[d1,d2,...,d N ]

[0078] Step 2.4 adjusts the distance measurement value by gradient descent method;

[0079] Use the RIC algorithm to adjust the distance vector through the gradient descent method and define the error function Represents the distance vector d Chan The real distance d true The mean square error between:

[0080]

[0081] Among them, d Chan,i is the distance vector of base station i after conversion by Chan algorithm, d true,i is the real distance of base station i;

[0082] Calculate the error function Relative to the distance vector d Chan Gradient

[0083]

[0084] Using gradient descent, adjust the distance measurement d Chan , the update formula is:

[0085]

[0086] in, is the distance vector of the kth iteration, α is the learning rate;

[0087] Set the number of iterations to K. After K iterations, the final output result vector is:

[0088]

[0089] in, is the gradient at the kth iteration.

[0090]

[0091] The distance measurement value is recursively and iteratively corrected through the RIC algorithm;

[0092] Step 3: Least squares solution;

[0093] The final position of the target is calculated using the least squares method. The actual position of the target node is X = (x, y, z), and the coordinates of each base station are p i =(x i ,y i ,z i ), the corrected distance vector is d RIC ;

[0094] For each base station i, there is the following distance equation:

[0095]

[0096] Select the reference base station p1 as the reference point with coordinates (x1, y1, z1), and then construct the following linear equations for the remaining base stations:

[0097]

[0098] Subtracting the two equations, we get:

[0099]

[0100] The above equations are expressed as a matrix system of linear equations:

[0101] AX=b

[0102] Among them, the matrix A and vector b are:

[0103]

[0104] Solve the linear equation system AX = b and get the target position X = (x, y, z)

[0105] X=(A T A) -1 A T b

[0106] The beneficial effects of adopting the above technical solution are:

[0107] The present invention provides a high-precision UWB positioning method for TDOA scenarios. The UKF effectively filters out measurement noise, the FNN corrects nonlinear errors, the Chan algorithm converts distance differences into absolute distances to improve calculation efficiency, and the RIC algorithm reduces errors through recursive correction, and finally accurately solves the target position through the least squares method. This system greatly enhances the stability of the UWB system in various complex scenarios, reduces the impact of multipath effects and signal interference on positioning accuracy, and reduces calculation complexity, making it more suitable for high-precision positioning demand scenarios, such as indoor positioning, autonomous driving, and intelligent navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0108] Figure 1 An overall flow chart provided for an embodiment of the present invention;

[0109] Figure 2 A two-dimensional TOA system positioning diagram provided by an embodiment of the present invention;

[0110] Figure 3 A feedforward neural network model diagram based on TDOA distance difference vector provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0111] The specific implementation of the present invention is further described in detail below in conjunction with the accompanying drawings and examples. The following examples are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0112] A high-precision UWB positioning method for TDOA scenarios, such as Figure 1 As shown, the following steps are included:

[0113] Step 1: Obtain the distance difference vector Δd between each UWB node;

[0114] In the UWB positioning system under the TDOA scenario, it is necessary to determine the location of the tag by measuring the time difference of the signal arriving at different base stations. First, multiple base stations are arranged in the three-dimensional positioning area. The UWB tag transmits the signal, and the base station receives these signals and records the reception time. Each base station independently records the timestamp of the signal it receives. N UWB receiving nodes are set in the three-dimensional positioning area. The UWB receiving nodes record the time of arrival of the same signal, calculate the distance difference based on the time difference, and obtain the distance difference vector.

[0115] Assume that the time when the i-th receiving node receives the signal is t i , the time when the jth receiving node receives the signal is t j , then the time difference Δt ij for:

[0116] Δt ij =t j -ti

[0117] The corresponding distance difference is:

[0118] Δd ij =c·Δt ij

[0119] Where c is the speed of light and the distance difference vector is:

[0120] Δd=[Δd 12 ,Δd 13 ,...,Δd 1N ,Δd 23 ,Δd 24 ,...,Δd 2N ,...,Δd (N-1)N ] T

[0121] Where, Δd ij Represents the distance difference between the i-th receiving node and the j-th receiving node; the distance difference vector includes the distance differences between all node pairs, with a total of N(N-1) / 2 elements.

[0122] Step 2: Propose the UKF-FNN-Chan-RIC algorithm for TDOA scenarios;

[0123] Step 2.1: Perform unscented Kalman filtering on the obtained distance difference vector;

[0124] In the unscented Kalman filter UKF, the state variable x of the target is k represents the position of the target in three-dimensional space at time k, where X k ,Y k ,Z k Respectively represent the coordinates of the target in three-dimensional space, namely:

[0125] x k =[X k ,Y k ,Z k ] T

[0126] UKF estimates the target position through the state transfer equation and observation equation;

[0127] The state transfer equation is:

[0128] x k+1 =f(x k )+w k

[0129] Among them, x k+1 is the state vector of the target at time k+1, f(x k) is the state transfer function, w k is the process noise; the observation equation is:

[0130] Z k =h(x k )+V k

[0131] Among them, Z k is the observation vector obtained at time k, h(x k ) is the observation model, and the target state variable x k Replaced with the predicted distance difference vector, V k is the observation noise;

[0132] The observation model h(x k ) Calculate the target state variable x k The distances between the receiving nodes are calculated and the differences between these distances are used to form the predicted distance difference vector h(x k );

[0133]

[0134] Where X i , Y i , Z i is the known three-dimensional coordinate of the i-th receiving node, that is, the fixed position of these nodes in space. This formula calculates the Euclidean distance from the target position to each base station.

[0135] UKF processes nonlinear systems through unscented transformation: First, the sigma point is calculated:

[0136]

[0137] Among them, χ k is the set of sigma points, P k is the state covariance matrix, L is the dimension of the state variables, and λ is the adjustment parameter, where λ = α 2 (L+κ)-L, α and κ are UKF parameters;

[0138] The sigma point is predicted by the state transfer equation, and the set of predicted sigma points is χ k+1|k :

[0139] χ k+1|k =f(χ k )

[0140] Calculate the predicted state mean and covariance P k+1 / k They are:

[0141]

[0142] Among them, W i m and W i c is the weight of the sigma point, It is the result after the i-th sigma point passes through the state transfer function, and Q is the covariance matrix of the process noise.

[0143] Predicted observation value Z k+1 for:

[0144] Z k+1 =h(χ k+1 |k)

[0145] Calculate the predicted observed mean and covariance P zz :

[0146]

[0147] in, is the result after the i-th sigma point passes through the observation model, and R is the covariance matrix of the observation noise.

[0148] Calculate the covariance matrix P between the state and the observation xz :

[0149]

[0150] The Kalman gain is K:

[0151]

[0152] Update the estimate of the mean of the observations for:

[0153]

[0154] Among them, h(z k+1 ) is the observation model for the predicted observations.

[0155] The filtered distance difference vector is the updated mean of the observations:

[0156]

[0157] Step 2.2: Introduce the feedforward neural network FNN to correct the distance difference vector after UKF filtering, such as Figure 3 As shown;

[0158] Since the distance difference vector Δd is obtained after UKF filtering filteredcontains nonlinear system errors, which can be modeled and corrected by FNN. The structure of FNN consists of an input layer, a hidden layer, and an output layer; the input of the input layer is the filtered distance difference vector Δd filtered The hidden layer processes the output of the previous layer through a nonlinear activation function, and the output layer outputs the corrected distance difference vector Δd corrected ; Assume that the dimension of the distance difference vector is p, then the number of neurons in the input layer and the output layer is p;

[0159] The FNN includes two hidden layers, specifically a first hidden layer and a second hidden layer; the first hidden layer has h1 neurons, and the second hidden layer has h2 neurons; for the first hidden layer, the output of the jth neuron for:

[0160]

[0161] in, represents the weight between the i-th element of the input vector and the j-th neuron in the first hidden layer, which is the weight parameter of the neuron; Δd filtered,i Denotes Δd filtered The i-th element of is the i-th component of the distance difference vector after the unscented Kalman filter processing. The bias term of the jth neuron in ; the bias term allows the model to output non-zero values ​​even when there is no input signal, which improves the flexibility of the model; σ is the activation function;

[0162] For the second hidden layer, the output of the kth neuron for:

[0163]

[0164] in, represents the weight connecting the jth neuron in the first hidden layer to the kth neuron in the second hidden layer; is the bias vector of the kth neuron in the second hidden layer;

[0165] The output of the lth neuron in the output layer is:

[0166]

[0167] Where, Δd filtered,l represents the lth component of the corrected distance difference vector, represents the weight connecting the lth neuron in the second hidden layer to the kth neuron in the output layer, is the bias of the lth neuron in the output layer.

[0168] Through FNN training, minimize the corrected distance difference vector Δd corrected The difference vector Δd from the actual distance true The error between . Use mean square error MSE as the loss function:

[0169]

[0170] Among them, C is the loss function value, which represents the corrected distance difference vector Δd corrected And the distance difference vector Δd after UKF filtering filtered The error between correctrd,l represents the lth component of the corrected distance difference vector, Δd filtered,l represents the lth component of the distance difference vector after UKF filtering;

[0171] The weights and biases of the network are adjusted through the back-propagation algorithm to optimize the loss function, so that the FNN can effectively correct the nonlinear error. The trained FNN outputs the corrected distance difference vector Δd corrected ;

[0172] Step 2.3: Use Chan algorithm to convert the distance difference measurement value into the distance measurement value;

[0173] Among the algorithms for TDOA scenarios, the positioning algorithm based on implicit function solution has poor stability, and implicit function solution consumes a lot of computing time. However, the Chan algorithm can transform the implicit function solution problem of TDOA into the explicit solution problem of TOA through one-step estimation. That is, the Chan algorithm can transform the distance difference measurement value into the distance measurement value for calculation, so that the positioning result has better stability and computing efficiency. Figure 2 shown.

[0174] In the UWB positioning system, the distance difference vector Δd is obtained by UKF filtering and FNN correction. corrected It can be further converted into an absolute distance vector through the Chan algorithm to improve the stability and calculation efficiency of positioning.

[0175] In order to calculate the absolute distance vector, the distance difference vector Δd needs to be corrected Convert to an absolute distance vector. First, from the distance difference vector Δd corrected Extract the partial vector ΔD:

[0176] ΔD=[Δd 21 ,Δd 31 ,...,Δd N1 ]

[0177] The equation is constructed using the distance difference between base stations:

[0178] Δdi1 =d i -d1

[0179] Δd i1 Represents the distance d from base station i to the target node i The distance difference between the reference base station and the target node d1; this difference is used in TDOA positioning and is the relative distance difference between the target node and different base stations. i is the actual distance from base station i to the target node, d1 is the actual distance from the reference base station to the target node;

[0180] After squaring, we get:

[0181]

[0182] Taking the reference base station as the origin, the distance of each base station relative to the reference base station is obtained:

[0183]

[0184] where p i represents the position vector of base station i, p1 represents the position vector of the reference base station, ||p i -p1|| represents the Euclidean distance between base station i and the reference base station, i = 2, 3, ..., N;

[0185] Therefore, for each base station, the distance vector d calculated using the distance difference equation is Chan It is expressed as:

[0186] d Chan =[d1,d2,...,d N ]

[0187] Step 2.4 adjusts the distance measurement value by gradient descent method;

[0188] Use the RIC algorithm to adjust the distance vector through gradient descent to reduce the error. Define the error function Represents the distance vector d Chan The real distance d true The mean square error between:

[0189]

[0190] Among them, d Chan,i is the distance vector of base station i after conversion by Chan algorithm, d true,i is the real distance of base station i;

[0191] Calculate the error function Relative to the distance vector d Chan Gradient

[0192]

[0193] Using gradient descent, adjust the distance measurement d Chan , to minimize the error function The update formula is:

[0194]

[0195] in, is the distance vector of the kth iteration, α is the learning rate;

[0196] Set the number of iterations to K. After K iterations, the final output result vector is:

[0197]

[0198] in, is the gradient at the kth iteration.

[0199]

[0200] Through the RIC algorithm, the distance measurement value is recursively and iteratively corrected to gradually reduce the error function The corrected distance vector d RIC , which is closer to the true distance vector d true .

[0201] Step 3: Least squares solution;

[0202] After obtaining the distance vector d after the RIC algorithm correction RIC , the least squares method is used to calculate the final position of the target. The actual position of the target node is X = (x, y, z), and the coordinates of each base station are p i =(x i ,y i ,z i ), the corrected distance vector is d RIC ;

[0203] For each base station i, there is the following distance equation:

[0204]

[0205] Select the reference base station p1 as the reference point with coordinates (x1, y1, z1), and then construct the following linear equations for the remaining base stations:

[0206]

[0207] Subtracting the two equations, we get:

[0208]

[0209] The above equations are expressed as a matrix system of linear equations:

[0210] AX=b

[0211] Among them, the matrix A and vector b are:

[0212]

[0213]

[0214] Solve the linear equation system AX = b and get the target position X = (x, y, z)

[0215] X=(A T A) -1 A T b.

[0216] The above description is only a preferred embodiment of the present disclosure and an explanation of the technical principles used. Those skilled in the art should understand that the scope of the invention involved in the embodiments of the present disclosure is not limited to the technical solutions formed by a specific combination of the above-mentioned technical features, but should also cover other technical solutions formed by any combination of the above-mentioned technical features or their equivalent features without departing from the above-mentioned inventive concept. For example, the above-mentioned features are replaced with the technical features with similar functions disclosed in the embodiments of the present disclosure (but not limited to) to form a technical solution.

Claims

1. A high-precision UWB positioning method for TDOA scenarios, characterized in that: The following steps are involved: Step 1: Obtain the distance difference vector Δd between each UWB node; Step 2: Propose the UKF-FNN-Chan-RIC algorithm for TDOA scenarios; Step 3: Use the least squares method to solve and obtain the target position.

2. A high-precision UWB positioning method for TDOA scenarios according to claim 1, characterized in that: The step 1 specifically includes: arranging multiple base stations in the three-dimensional positioning area, the UWB tag transmits signals, the base stations receive these signals and record the reception time, and each base station independently records the timestamp of the signal it receives; N UWB receiving nodes are set in the three-dimensional positioning area. The UWB receiving nodes record the time when the same signal arrives, calculate the distance difference based on the time difference, and obtain the distance difference vector; Assume that the time when the i-th receiving node receives the signal is t i , the time when the jth receiving node receives the signal is t j , then the time difference Δt ij for: Δt ij =t j -t i The corresponding distance difference is: Δd ij =c·Δt ij Where c is the speed of light and the distance difference vector is: Δd=[Δd 12 ,Δd 13 ,...,Δd 1N ,Δd 23 ,Δd 24 ,...,Δd 2N ,...,Δd (N-1)N ] T Where, Δd ij Represents the distance difference between the i-th receiving node and the j-th receiving node; the distance difference vector includes the distance differences between all node pairs, with a total of N(N-1) / 2 elements.

3. A high-precision UWB positioning method for TDOA scenarios according to claim 2, characterized in that: The step 2 comprises the following steps: Step 2.1: Perform unscented Kalman filtering on the obtained distance difference vector; In the unscented Kalman filter UKF, the state variable x of the target is k represents the position of the target in three-dimensional space at time k, where X k ,Y k ,Z k Respectively represent the coordinates of the target in three-dimensional space, namely: x k =[X k ,Y k ,Z k ] T UKF estimates the target position through the state transfer equation and observation equation; The state transfer equation is: x k+1 =f(x k )+w k Among them, x k+1 is the state vector of the target at time k+1, f(x k ) is the state transfer function, w k is the process noise; The observation equation is: Z k =h(x k )+V k Among them, Z k is the observation vector obtained at time k, h(x k ) is the observation model, and the target state variable x k Replaced with the predicted distance difference vector, V k is the observation noise; The observation model h(x k ) Calculate the target state variable x k The distances between the receiving nodes are calculated and the differences between these distances are used to form the predicted distance difference vector h(x k ); Where X i , Y i , Z i is the known three-dimensional coordinate of the i-th receiving node, that is, the fixed position of these nodes in space. This formula calculates the Euclidean distance from the target location to each base station; UKF processes nonlinear systems through unscented transformation: First, the sigma point is calculated: Among them, χ k is the set of sigma points, P k is the state covariance matrix, L is the dimension of the state variables, and λ is the adjustment parameter, where λ = α 2 (L+κ)-L, α and κ are UKF parameters; The sigma point is predicted by the state transfer equation, and the set of predicted sigma points is χ k+1|k : x k+1|k =f(x k ) Calculate the predicted state mean and covariance P k+1 / k They are: Among them, W i m and W i c is the weight of the sigma point, is the result after the i-th sigma point passes through the state transfer function, Q is the covariance matrix of the process noise; Predicted observation value Z k+1 for: Z k+1 =h(χ k+1 |k) Calculate the predicted observed mean and covariance P zz : in, is the result after the i-th sigma point passes through the observation model, and R is the covariance matrix of the observation noise; Calculate the covariance matrix P between the state and the observation xz : The Kalman gain is K: Update the estimate of the mean of the observations for: Among them, h(z k+1 ) is the observation model of the predicted observations; The filtered distance difference vector is the updated mean of the observations: Step 2.2: Introduce the feedforward neural network FNN to correct the distance difference vector after UKF filtering; The structure of the feedforward neural network FNN consists of an input layer, a hidden layer and an output layer; the input of the input layer is the filtered distance difference vector Δd filtered The hidden layer processes the output of the previous layer through a nonlinear activation function, and the output layer outputs the corrected distance difference vector Δd corrected ; Assume that the dimension of the distance difference vector is p, then the number of neurons in the input layer and the output layer is p; The FNN includes two hidden layers, specifically a first hidden layer and a second hidden layer; the first hidden layer has h1 neurons, and the second hidden layer has h2 neurons; for the first hidden layer, the output of the jth neuron for: in, represents the weight between the i-th element of the input vector and the j-th neuron in the first hidden layer, which is the weight parameter of the neuron; Δd filtered,i Denotes Δd filtered The i-th element of The bias term of the jth neuron in , σ is the activation function; For the second hidden layer, the output of the kth neuron for: in, represents the weight connecting the jth neuron in the first hidden layer to the kth neuron in the second hidden layer; is the bias vector of the kth neuron in the second hidden layer; The output of the lth neuron in the output layer is: Where, Δd filtered,l represents the lth component of the corrected distance difference vector, represents the weight connecting the lth neuron in the second hidden layer to the kth neuron in the output layer, is the bias of the lth neuron in the output layer; Through FNN training, minimize the corrected distance difference vector Δd corrected The difference vector Δd from the actual distance true The error between; using mean square error MSE as the loss function: Among them, C is the loss function value, which represents the corrected distance difference vector Δd corrected And the distance difference vector Δd after UKF filtering filtered The error between correctrd,l represents the lth component of the corrected distance difference vector, Δd filtered,l represents the lth component of the distance difference vector after UKF filtering; The weights and biases of the network are adjusted through the back-propagation algorithm to optimize the loss function. The trained FNN outputs the corrected distance difference vector Δd corrected ; Step 2.3: Use Chan algorithm to convert the distance difference measurement value into the distance measurement value; The distance difference vector Δd needs to be corrected Convert to absolute distance vector; first, from the distance difference vector Δd corrected Extract the partial vector ΔD: ΔD=[Δd 21 ,Δd 31 ,...,Δd N1 ] The equation is constructed using the distance difference between base stations: Δd i1 =d i -d1 Δd i1 Represents the distance d from base station i to the target node i The distance difference between the reference base station and the target node d1, d i is the actual distance from base station i to the target node, d1 is the actual distance from the reference base station to the target node; After squaring, we get: Taking the reference base station as the origin, the distance of each base station relative to the reference base station is obtained: where p i represents the position vector of base station i, p1 represents the position vector of the reference base station, ||p i -p1|| represents the Euclidean distance between base station i and the reference base station, i = 2, 3, ..., N; Therefore, for each base station, the distance vector d calculated using the distance difference equation is Chan It is expressed as: d Chan =[d1,d2,...,d N ] Step 2.4 adjusts the distance measurement value by gradient descent method; Use the RIC algorithm to adjust the distance vector through the gradient descent method and define the error function Represents the distance vector d Chan The real distance d true The mean square error between: Among them, d Chan,i is the distance vector of base station i after conversion by Chan algorithm, d true,i is the real distance of base station i; Calculate the error function Relative to the distance vector d Chan Gradient Using gradient descent, adjust the distance measurement d Chan , the update formula is: in, is the distance vector of the kth iteration, α is the learning rate; Set the number of iterations to K. After K iterations, the final output result vector is: in, is the gradient of the kth iteration; The distance measurement value is recursively and iteratively corrected using the RIC algorithm.

4. A high-precision UWB positioning method for TDOA scenarios according to claim 3, characterized in that: The step 3 is specifically as follows: the final position of the target is calculated using the least squares method. The real position of the target node is X=(x, y, z). The coordinates of each base station are p i =(x i ,y i ,z i ), the corrected distance vector is d RIC ; For each base station i, there is the following distance equation: Select the reference base station p1 as the reference point with coordinates (x1, y1, z1), and then construct the following linear equations for the remaining base stations: Subtracting the two equations, we get: The above equations are expressed as a matrix system of linear equations: AX=b Among them, the matrix A and vector b are: Solve the linear equation system AX = b and get the target position X = (x, y, z) X=(A T A) -1 A T b。

Images