CN110308419B

CN110308419B - Robust TDOA (time difference of arrival) positioning method based on static solution and particle filtering

Info

Publication number: CN110308419B
Application number: CN201910565034.8A
Authority: CN
Inventors: 申富饶; 周至; 李俊; 安俊逸
Original assignee: Jiangsu Donn Technology Co ltd; Nanjing University
Current assignee: Jiangsu Donn Technology Co ltd; Nanjing University
Priority date: 2019-06-27
Filing date: 2019-06-27
Publication date: 2021-04-06
Anticipated expiration: 2039-06-27
Also published as: CN110308419A

Abstract

The invention discloses a robust TDOA (time difference of arrival) positioning method based on static solution and particle filtering, which comprises the following steps: step 1, reading a TDOA vector; step 2, estimating the current coordinates, the motion speed and the motion direction of the target according to the historical information; step 3, carrying out anomaly detection on the TDOA vector by using state estimation, and deleting an abnormal value; step 4, if the number of the residual base stations is less than 3, the coordinate estimation is used as a target current coordinate, if the number of the residual base stations is equal to 3, the target current coordinate is solved through a simultaneous equation, and if the number of the residual base stations is greater than 3, the coordinate estimation is used as an initial value of a Taylor algorithm to solve the target current coordinate; step 5, inputting the obtained coordinates into a limited particle filter algorithm to obtain a final positioning result; and 6, finishing the processing of the TDOA vector, and returning to the step 1 if data are still to be input.

Description

Robust TDOA (time difference of arrival) positioning method based on static solution and particle filtering

Technical Field

The invention relates to an indoor positioning technology, in particular to a robust TDOA (time difference of arrival) positioning method based on static solution and particle filtering.

Background

With the rapid development of wireless network technology and the continuous increase of the demand of internet of things, location-based services are receiving more and more attention and research in all social circles, and are widely applied to various fields such as target monitoring, warehouse logistics, business intelligence and the like, so that great convenience is brought to life and production. Traditional positioning technologies, such as GPS in the united states, galileo in europe, and beidou satellite navigation system in china, have achieved great success in outdoor scenes due to their wide coverage, high accuracy, and low cost. However, in an indoor scenario, the applicability of the above positioning technique is greatly challenged due to the inability of satellite signals to penetrate buildings, and the presence of a large number of obstacles in the room for shielding and reflection. In order to solve this problem, researchers have proposed devices such as infrared, bluetooth, ultra wideband, and radio frequency identification to perform indoor positioning, and have proposed various positioning algorithms according to the used principle, wherein a time difference positioning method, i.e., tdoa (time difference of arrival), is a common positioning principle.

TDOA location algorithms can be largely classified into two categories, depending on whether the motion state of the target is taken into account: the former algorithm, which calculates the position of the target using only ranging information at the current time, and the latter algorithm, which uses state information of the target at the past time. One commonly used static TDOA location algorithm is the Taylor algorithm, whose basic idea is to perform Taylor expansion around some initial coordinate and ignore higher order components, then optimize the coordinate by calculating a local least squares solution of the error, and iteratively perform the above optimization process until the coordinate converges, and output the final location result. Under the conditions that the initial coordinate is close to the real coordinate and the data noise is small, the Taylor algorithm can obtain a relatively accurate result, but in practical application, the selection of the initial value is difficult, and the noise in the real environment is usually large, so that a large error is brought to the Taylor algorithm. The literature: foy W H.position-Location Solutions by Taylor-Series Estimation [ J ]. IEEE Transactions on Aerospace & Electronic Systems,2007, AES-12(2): 187-.

The dynamic TDOA (time difference of arrival) positioning algorithm generally refers to various filtering algorithms, mainly including extended Kalman filtering, lossless Kalman filtering and particle filtering, and the algorithms can model noise and perform combined positioning according to the state of a target at the past moment and current ranging information, so that the performance of the algorithms in a noisy environment is generally superior to that of a static method. Fredrik Gunnarsson proposes a classical TDOA location algorithm based on particle filtering, and the basic idea is to calculate the weight of each particle directly according to the received TDOA vector and iteratively execute four steps of a sampling importance resampling algorithm until the output state converges to obtain the final location result. Although the algorithm has certain robustness to noise, under a real environment, the positioning accuracy of the algorithm is still remarkably reduced due to the existence of various complex noises. The literature: gustafsson F, Gunnasson F. localization using time-differences of arrival measures [ C ]//2003IEEE International Conference on Acoustics, Speech, and Signal Processing,2003.Proceedings. (ICASSP'03). IEEE,2003,6: VI-553.

Disclosure of Invention

The purpose of the invention is as follows: the problem that the accuracy of a traditional TDOA positioning algorithm is seriously reduced and even the positioning fails when various noises in a real environment face is solved, an exception handling strategy is provided to improve the data quality, a static algorithm and a dynamic algorithm are combined, the positioning error is greatly reduced in a layered solving mode, and the track smoothness is improved.

In order to solve the technical problem, the invention discloses a robust TDOA (time difference of arrival) positioning method based on static solution and particle filtering, which can be used for indoor navigation, target monitoring, industrial robots and other applications, and specifically comprises the following steps:

step 1, reading a TDOA vector;

step 2, estimating the coordinates, the movement speed and the movement direction of the target at the current moment according to the positioning result of the past moment;

step 3, carrying out anomaly detection on each dimension of data of the TDOA vector by using the coordinate, the motion speed and the motion direction of the target at the current moment, which are obtained by estimation in the step 2, and deleting an abnormal value in the data;

step 4, solving the current coordinates of the target;

and 5, inputting the current coordinates of the target into a limited particle filter algorithm to obtain a final positioning result.

In step 1, the read TDOA vector is a vector formed by TDOA data related to the position of the target at the current time, and the read TDOA vector is represented as input:

input＝(a21,a31,…,am1)，

where ai1 indicates the difference between the distance between the target current position and the ith base station and the distance between the target current position and the 1 st base station, i indicates the number of the slave base station, 2 ═ i ═ m, m indicates the number of base stations, and the 1 st base station is the master base station.

In step 2, the positioning results of past 3 moments t-1, t-2 and t-3 are obtained, the movement speeds of the targets at the moments are calculated by dividing the distance between the positioning results by the time difference, the movement speeds of the targets at the moments t-1, t-2 and t-3 are set as v (t-1), v (t-2) and v (t-3), respectively, the direction of a coordinate connecting line between the moment t-3 and the previous moment, namely the moment t-4 is set as the movement direction of the target at the moment t-3, similarly, the direction of the coordinate connecting line between the moment t-2 and the moment t-3 is set as the movement direction of the target at the moment t-2, the direction of the coordinate connecting line between the moment t-1 and the moment t-2 is set as the movement direction of the target at the moment t-1, and t-1, the moving directions of the target at the time t-2 and the time t-3 are d (t-1), d (t-2) and d (t-3), respectively, and the moving speed and the moving direction of the target at the current time are estimated by adopting the following formulas:

v＝(v(t-1)+v(t-2)+v(t-3))/3，

d＝(d(t-1)+d(t-2)+d(t-3))/3，

where v represents the motion velocity estimation of the target at the current time, and d represents the motion direction estimation of the target at the current time. And setting the time interval between the current time and the previous time as T, and obtaining the movement distance of the target as deltas-v T. Setting the coordinates of the target at the previous moment to be (x ', y'), and estimating the coordinates of the target at the current moment as follows:

xp＝x’+△s*cos(d)，

yp＝y’+△s*sin(d)，

where xp represents the abscissa estimation of the target at the current time, yp represents the ordinate estimation of the target at the current time, cos (d) represents the cosine value in the direction d, sin (d) represents the sine value in the direction d, and (xp, yp) represents the coordinate estimation of the target at the current time.

In step 3, the TDOA data related to the ith base station at the previous time is set to ai 1', 2 ═ i ═ m, and then the formula of anomaly detection is:

|ai1-ai1’|<2△s，

where | ai1-ai1 '| represents the absolute value of ai1-ai 1', the above formula gives an upper limit to the amount of change in each dimension of the current TDOA data as compared to the previous time, and for TDOA data that does not satisfy the formula, it is removed as an outlier from the TDOA vector.

Step 4 comprises the following steps: and (3) regarding the TDOA vector after the abnormal value is deleted, if the number of the residual base stations is less than 3, using the coordinate estimation of the target at the current moment obtained in the step (2) as the current coordinate of the target, if the number of the residual base stations is equal to 3, solving the current coordinate of the target through a simultaneous equation, and if the number of the residual base stations is more than 3, using the coordinate estimation of the target at the current moment as the initial value of the Taylor algorithm to solve the current coordinate of the target.

In step 4, since step 3 may delete several outliers from the received TDOA vector, i.e. delete TDOA data associated with several base stations, different processing needs to be performed for different numbers of remaining base stations. For the TDOA vector after the abnormal value is deleted, if the number of the residual base stations is less than 3, namely the dimension of the TDOA vector after the abnormal value is deleted is 1 or null, the solution cannot be carried out at the moment, and in order to avoid losing the position of the target at the current moment, the coordinate estimation (xp, yp) obtained in the step 2 is used as the target current coordinate. If the number of remaining base stations is equal to 3, namely the dimension of the TDOA vector after deleting the abnormal value is 2, the numbers of the remaining base stations are set as u and v, namely the 2-dimensional data are au1 and av1 respectively, 2< u < ═ m and 2< v < > m, and then the equations related to au1 and av1 are as follows:

sqrt((xsu-x)^2+(ysu-y)^2)-sqrt((xs1-x)^2+(ys1-y)^2)＝au1，

sqrt((xsv-x)^2+(ysv-y)^2)-sqrt((xs1-x)^2+(ys1-y)^2)＝av1，

wherein, (xsu, ysu) represents the coordinates of the base station u, (xsv, ysv) represents the coordinates of the base station v, (xs1, ys1) represents the coordinates of the master base station, (x, y) represents the target current coordinates to be solved, sqrt represents the power of the square, and ^2 represents the square, and the target current coordinates can be obtained by combining the above equations.

If the number of the residual base stations is more than 3, namely the dimensionality of the TDOA vector after the abnormal value is deleted is more than 2, the Taylor algorithm is used for solving, because the algorithm needs a more accurate initial value for iteration, the coordinate estimation (xp, yp) is used as the initial value, and therefore the current target coordinate is obtained.

In step 5, the limited particle filter algorithm is a particle filter algorithm designed based on a sampling importance resampling algorithm, and the basic idea is to simulate a large number of particles, each particle has a state and a weight, and the state distribution and the weight of all the particles simulate the probability distribution of the real position of the target. The algorithm takes the coordinates obtained in the step 4 as input, outputs the adjusted final coordinates, and the flow comprises four basic stages of prediction, updating, outputting and resampling, and specifically comprises the following steps:

step 5-1, prediction stage: setting the state of the particle at the time k to be (xk, yk, vk, dk), where xk, yk, vk, dk respectively represent the abscissa, ordinate, moving speed and moving direction of the particle at the time k, the prediction formula is:

vk＝v(k-1)+T*wv，

dk＝d(k-1)+T*wd，

xk＝x(k-1)+wv/wd*sin(dk)*T+v(k-1)/wd*(sin(dk)-sin(d(k-1)))+wv/(wd^2)*(cos(dk)-cos(d(k-1)))，

yk＝y(k-1)-wv/wd*cos(dk)*T-v(k-1)/wd*(cos(dk)-cos(d(k-1)))+wv/(wd^2)*(sin(dk)-sin(d(k-1)))，

wherein x (k-1), y (k-1), v (k-1) and d (k-1) respectively represent the abscissa, the ordinate, the motion speed and the motion direction of the particle at the moment of k-1; wv denotes the rate of change of velocity, which follows a gaussian distribution with a mean value of 0 and a standard deviation of stdv, wd denotes the rate of change of angle, which follows a gaussian distribution with a mean value of 0 and a standard deviation of stdd, wv and wd can be understood as process noise in a particle filter system; t represents the time interval between time k and time k-1. The above prediction formula gives the way to derive the state of the particle at time k from the state of the particle at time k-1, i.e. wv and wd are first randomly generated, followed by the calculation of vk and dk, and on this basis xk and yk are calculated. In order to increase the constraint of the algorithm to meet specific requirements under some specific scenes, a re-prediction strategy is added in the prediction process, and the specific re-prediction strategy is related to the constraint conditions of the system, for example, when the system requires that the target speed is not more than 1m/s, the re-prediction strategy is that after the particle speed vk is calculated, whether the particle speed vk is more than the upper limit 1m/s is judged firstly, if so, the particle speed vk is considered to damage the speed constraint conditions, vk is regenerated until the particle speed vk meets the requirements less than 1m/s, and then dk, xk and yk are calculated; when the system requires the object to move in a closed area, such as a round stadium, the re-prediction strategy is to check whether the particle coordinate (xk, yk) is in the circle after calculating the coordinate, if the particle coordinate exceeds the boundary of the circle, the region constraint is considered to be damaged, wv and wd are regenerated, and vk, dk, xk and yk are calculated until the coordinate is in the circle.

Step 5-2, updating stage: calculating the weight of each particle according to the target current coordinate obtained in the step 4, and setting the target current coordinate as (x, y), wherein the calculation formula is as follows:

wk＝1/(sqrt(2*pi)*stdo)*exp(-((xk-x)^2+(yk-y)^2)/(2*(stdo^2)))，

where wk denotes the weight of the particle at time k, pi denotes the circumferential ratio, stdo is a standard deviation parameter, and exp (t) denotes the t-th power of the natural constant e.

Step 5-3, output stage: the prediction and update stage needs to be performed on all particles in the system, the number of the particles is set to be N, after the weights of all the particles are obtained, the particles are normalized, and the formula is as follows:

wk_(i)＝wk(i)/(wk(1)+wk(2)+…+wk(N))，

where wk _ (i) represents the normalized weight of the ith particle at time k, wk (i) represents the weight of the ith particle at time k, and 1< ═ i < ═ N. The final output target coordinates are:

x_＝wk_(1)*xk(1)+wk_(2)*xk(2)+…+wk_(N)*xk(N)，

y_＝wk_(1)*yk(1)+wk_(2)*yk(2)+…+wk_(N)*yk(N)，

where x _ represents the target abscissa of the final output, y _ represents the target ordinate of the final output, (x _, y _), i.e., the target coordinate of the final output of the algorithm at time k, xk (i) represents the abscissa of the ith particle at time k, yk (i) represents the ordinate of the ith particle at time k, and 1< ═ i < ═ N.

Step 5-4, resampling stage: and (4) carrying out importance resampling on the particles, regenerating N new particles, wherein the probability of resampling each particle is equal to the weight of each particle, and carrying out on the new particles when solving the positioning result at the next moment.

The invention also comprises step 6, the TDOA vector of the current moment is processed, if data are still to be input, the step 1 is returned.

Has the advantages that: the method has the obvious advantages that the probability of positioning failure can be reduced in various indoor scenes, particularly scenes containing a large number of obstacles and having serious shielding and reflecting phenomena, the positioning accuracy is effectively improved, and a stable and smooth motion track of the target is obtained.

Drawings

The foregoing and other advantages of the invention will become more apparent from the following detailed description of the invention when taken in conjunction with the accompanying drawings.

FIG. 1 is an overall flow chart of the present invention.

Fig. 2 is a flow chart of a limited particle filter algorithm in the present invention.

FIG. 3a is a comparison graph of the trajectory obtained by the present invention and several classical methods when the tag is held in the real scene.

Fig. 3b is a comparison graph of the trajectory obtained by the present invention and several classical methods when wearing a tag in a real scene.

Detailed Description

The invention is further explained below with reference to the drawings and the embodiments.

Fig. 1 is an overall flow chart of the present invention, comprising 6 steps.

In step 1, a TDOA vector is read, the dimension of the vector is the same as the number of slave base stations in the positioning system, and a total of m base stations are set, wherein the base station No. 1 is a master base station, and the base stations No. 2 to m are slave base stations, the TDOA vector is read in the form of input ═ (a21, a31, …, am1), wherein ai1 represents the difference between the distance between the target current position and the ith base station and the distance between the target current position and the master base station, i represents the number of the slave base station, and 2<, i < ═ m.

In step 2, estimating the state of the target at the current moment, wherein the state includes the motion speed, the motion direction and the coordinates of the target, and the purpose of this step is to provide support for the data denoising in the subsequent step 3 and the solving process in the step 4. Because the positioning is a continuous process, the positioning results of past moments exist at the moment, and the current movement speed and direction are firstly estimated according to the results, wherein the speed estimation mode is to average the speeds of a plurality of past moments, and the direction estimation mode is to average the directions of a plurality of past moments. The specific numerical value of "a few" here indicates the size of a time window, that is, how much historical information is utilized when estimating the current speed and direction, and the larger the value, the more the historical information is considered, the stronger the robustness to noise is, but the lower the accuracy is, the smaller the value is, the less the historical information is considered, the closer the estimated result is to the true value at the current time, but the more easily the estimated result is affected by abnormal data. In practical application, the window size is generally 3, at this time, the speeds at t-1, t-2, and t-3 are respectively v (t-1), v (t-2), and v (t-3), and the directions are respectively d (t-1), d (t-2), and d (t-3), so that the formula for estimating the moving speed and direction of the target at the current time is as follows:

v＝(v(t-1)+v(t-2)+v(t-3))/3，

d＝(d(t-1)+d(t-2)+d(t-3))/3，

where v represents the motion velocity estimate of the target at the current time and d represents the motion direction estimate of the target at the current time. The time interval between the current time and the previous time is set as T, the coordinates of the target at the previous time are set as (x ', y'), and after the speed and direction estimates are obtained, the speed is multiplied by time to obtain the movement distance Δ s ═ v × T, so that the abscissa estimate xp ═ x '+ Δs cos (d) and the ordinate estimate yp ═ y' + Δs sin (d) of the target at the current time are obtained, wherein cos (d) represents the cosine value of the direction d, and sin (d) represents the sine value of the direction d.

In step 3, denoising of the input TDOA vector is required, because a certain abnormal value may exist in the input data due to the existence of the environmental noise, and if denoising processing is not performed, the subsequent coordinate solving process may be seriously affected, the positioning accuracy is significantly reduced, and even the solving may fail. Setting the TDOA data associated with the ith base station at the previous time as ai1 ', 2< ═ i < ═ m, the formula for anomaly detection is | ai1-ai 1' | <2 Δ s, where | ai1-ai1 '| represents the absolute value of ai1-ai 1'. The equation is derived from the triangular relationship between the target motion trajectory and the base station location, which gives an upper bound on the amount of change in each dimension of the current TDOA vector compared to the previous time, and for those TDOA data that do not satisfy the equation, they are removed from the vector as outliers. It should be noted that the TDOA vector at the previous time is not directly received, because the environmental noise may cause a large error between the received data and its true value, and the reasonable TDOA vector should be obtained by reversely deducing the positioning result finally output by the algorithm at the previous time and the position of each base station.

In step 4, since several dimensional data may be deleted in the denoising process of the previous step, the dimension of the TDOA vector is uncertain. When the dimension is less than 2, namely only one base station or no base station is left, the information amount is too small, the coordinate cannot be solved, in order to avoid no result output, the coordinate estimation obtained in the step 2 is used as a positioning result, and although the coordinate estimation is not completely accurate, the situation of positioning failure can be effectively avoided. When the number of remaining base stations is 2, the traditional TDOA method based on least square cannot be applied, and can only be solved through the most basic simultaneous equations. When the number of the remaining base stations is more than 2, the information amount is relatively sufficient at the moment, the classical Taylor algorithm is adopted for solving, the algorithm needs a relatively accurate initial value to complete iteration, and the coordinate estimation obtained in the step 2 is taken as the initial value to ensure that the algorithm can be converged under most conditions and is taken as a positioning result when the algorithm cannot be converged.

In step 5, the positioning result obtained in the previous step is adjusted through a limited particle filter algorithm, the motion track is smoothed, and the final positioning result is output. The limited particle filter algorithm adds a particle constraint condition to the classical particle filter algorithm to meet specific requirements in specific scenes, for example, in some logistics warehouses, vehicles carrying goods have upper speed limits, and the positioning result must meet the requirement. Considering that the output of the particle filter is a weighted sum of all particle states, if a constraint is added to each particle, the final output must also satisfy this constraint. The flow chart of the algorithm is shown in fig. 2, which includes four basic stages of prediction, update, output, and resampling. In the prediction stage, for each particle, first, the change rates of the motion speed and direction, i.e. the acceleration and the angular acceleration, are randomly generated, and they respectively follow the gaussian distribution with the mean value of 0 and the standard deviation of stdv and stdd, which are two parameters and are generally set to be values close to the target real acceleration and angular acceleration, the more the particle state converges closer to the real value, if the deviation of the setting is larger, the initial stage of the system has a certain fluctuation, but the particle state still converges as time goes on. And then calculating the speed, the direction and the coordinates of the particle at the current moment, checking whether constraint conditions are met, and if the constraint conditions are not met, predicting again, namely regenerating the acceleration and the angular acceleration.

In the updating stage, the weight of each particle is calculated from the input positioning result of the previous step, and the basic idea is that the closer the particle is to the result, the higher the probability that the particle is in the target real position, the weight of the particle is equal to the probability and follows a gaussian distribution with a mean value of 0 and a standard deviation of stdo, stdo is a parameter which determines the smoothness and the delay of the trajectory, the larger stdo is, the smoother the motion trajectory is, but the greater the delay is, i.e. the more the positioning result obtained at each moment lags behind the real position thereof, the smaller stdo is, the poorer the smoothness of the motion trajectory is, but the smaller the delay is. The setting of the parameter depends on the specific application scenario, and in case of a large environmental noise in the scenario, it is recommended to set a large value, otherwise it is recommended to set 0.1 m.

In the output stage, the weights of all particles are normalized, and the final state is output through weighted summation.

In the resampling stage, N particles are regenerated, the particles with higher weight in the system are reserved, the particles with lower weight are removed, and the probability of resampling each particle is equal to the weight of each particle. The particle number N determines the fitting degree of the probability density function, the larger the N is, the better the fitting is, the better the algorithm effect is, but the larger the calculation amount is. Generally, setting N between 1000 and 5000 ensures a reasonable time overhead while ensuring the effectiveness of the algorithm.

In step 6, the TDOA data at the current moment is processed, the obtained result is used for displaying the motion track, and if data are still to be input, the step 1 is returned.

Examples

In order to verify the effectiveness of the proposed method, a UWB positioning system was deployed in a closed room, and the proposed method of the present invention (i.e., the ourmethod in fig. 3a and 3 b) and 3 classical algorithms were tested in comparison, the 3 methods including the Taylor algorithm, the 2WLS algorithm, and the particle filtering algorithm PF (i.e., Taylor, 2WLS, PF in fig. 3a and 3 b). The room size is 6m, with 4 columns, pieces of furniture and two large pieces of glass to simulate the harsh conditions of shadowing, reflection, strong gaussian noise, etc., with 4 base stations deployed in 4 corners of the room. The testers respectively hold the hands and wear the tags to walk in the room, and two groups of real TDOA data are acquired. The collected data sequence is used as test data to be calculated in the invention, wherein the implementation and parameter details of each step are as follows:

step 1, reading a TDOA vector;

step 2, estimating the current coordinates, the motion speed and the motion direction of the target according to the historical information, wherein the size of a time window when the speed and the direction are estimated is set to be 3;

step 3, carrying out anomaly detection on the TDOA vector by using state estimation, and deleting an abnormal value;

step 4, solving the current coordinates of the target according to different residual base station numbers;

step 5, inputting the obtained coordinates into a limited particle filter algorithm to obtain a final positioning result, wherein the number N of particles is set to 5000, and standard deviation parameters stdv, stdd and stdo are respectively set to 0.16m/(s ^2), 8rad/(s ^2) and 0.1 m;

and 6, finishing the processing of the TDOA vector, and returning to the step 1 if data are still to be input.

Fig. 3a is a graph showing the trajectory calculated by the present invention and 3 classical methods for a TDOA data sequence acquired while holding a tag, and fig. 3b is a graph showing the trajectory calculated by the present invention and 3 classical methods for a TDOA data sequence acquired while wearing a tag. In a handheld label scene, the upper part of the real track is close to a straight line y which is 8, and the right part is close to a straight line x which is 4, so that the 2WLS algorithm is completely failed, the track obtained by the particle filter algorithm (i.e. the PF algorithm in the figure) is seriously distorted, and the result of the Taylor algorithm is better than the two algorithms, but the track has obvious jitter and smoothness is not as good as that of the invention. In addition, in the upper right corner of the track, the Taylor algorithm does not calculate the coordinates for a period of time, and the track obtained by the method has good continuity, which shows that the robustness is stronger. In a scene of wearing the label, the real track is in a spiral shape, because the wireless signal can be shielded by a body, the noise of the group of data is larger, and as can be seen easily from an effect comparison graph, the Taylor algorithm, the 2WLS algorithm and the particle filtering algorithm are completely invalid, and the track obtained by the method has good stability and smoothness. The two groups of tests prove the effectiveness of the method in a real scene, and particularly in a complex environment, the performance of the method is superior to that of the traditional method.

The invention provides a robust TDOA positioning method based on static solution and particle filtering, and a plurality of methods and approaches for implementing the technical solution are provided, the above description is only a preferred embodiment of the invention, and it should be noted that, for those skilled in the art, a plurality of improvements and modifications may be made without departing from the principle of the invention, and these improvements and modifications should also be regarded as the protection scope of the invention. All the components not specified in the present embodiment can be realized by the prior art.

Claims

1. A robust TDOA positioning method based on static solution and particle filtering is characterized by comprising the following steps:

step 1, reading a TDOA vector;

step 4, solving the current coordinates of the target;

2. The method as recited in claim 1, wherein in step 1, the TDOA vector read is a vector of TDOA data related to the location of the target at the current time, and the TDOA vector read is represented as input:

input＝(a21,a31,…,am1)，

3. The method according to claim 2, wherein in step 2, the positioning results of the past 3 times t-1, t-2 and t-3 are obtained, the moving speeds of the targets at these times are calculated by dividing the distance between them by the time difference, the moving speeds of the targets at the times t-1, t-2 and t-3 are respectively set as v (t-1), v (t-2) and v (t-3), the direction of the coordinate connecting line between the time t-3 and the previous time, i.e. the time t-4, is taken as the moving direction of the target at the time t-3, and similarly, the direction of the coordinate connecting line between the time t-2 and the time t-3 is taken as the moving direction of the target at the time t-2, and the direction of the coordinate connecting line between the time t-1 and the time t-2 is taken as the moving direction of the target at the time t-1, setting the moving directions of the target at t-1, t-2 and t-3 as d (t-1), d (t-2) and d (t-3), and estimating the moving speed and direction of the target at the current time by adopting the following formulas:

v＝(v(t-1)+v(t-2)+v(t-3))/3，

d＝(d(t-1)+d(t-2)+d(t-3))/3，

wherein v represents the motion velocity estimation of the target at the current moment, and d represents the motion direction estimation of the target at the current moment; setting the time interval between the current moment and the previous moment as T, and obtaining the movement distance of the target as deltas ═ v × T; setting the coordinates of the target at the previous moment to be (x ', y'), and estimating the coordinates of the target at the current moment as follows:

xp＝x’+△s*cos(d)，

yp＝y’+△s*sin(d)，

4. The method of claim 3, wherein in step 3, the TDOA data associated with the ith base station at the previous time is set to ai 1', and the formula for anomaly detection is as follows:

|ai1-ai1’|<2△s，

5. The method of claim 4, wherein step 4 comprises: and (3) regarding the TDOA vector after the abnormal value is deleted, if the number of the residual base stations is less than 3, using the coordinate estimation of the target at the current moment obtained in the step (2) as the current coordinate of the target, if the number of the residual base stations is equal to 3, solving the current coordinate of the target through a simultaneous equation, and if the number of the residual base stations is more than 3, using the coordinate estimation of the target at the current moment as the initial value of the Taylor algorithm to solve the current coordinate of the target.

6. The method as claimed in claim 5, wherein in step 4, if the number of remaining base stations is equal to 3, i.e. the dimension of the TDOA vector after removing outliers is 2, the numbers of the remaining base stations are set as u and v, i.e. the 2-dimensional data are au1 and av1, 2< ═ u < ═ m and 2< ═ v < > m respectively, and then the equations related to au1 and av1 are:

sqrt((xsu-x)^2+(ysu-y)^2)-sqrt((xs1-x)^2+(ys1-y)^2)＝au1，

sqrt((xsv-x)^2+(ysv-y)^2)-sqrt((xs1-x)^2+(ys1-y)^2)＝av1，

7. The method of claim 6, wherein step 5 comprises:

vk＝v(k-1)+T*wv，

dk＝d(k-1)+T*wd，

wherein x (k-1), y (k-1), v (k-1) and d (k-1) respectively represent the abscissa, the ordinate, the motion speed and the motion direction of the particle at the moment of k-1; wv denotes the rate of change of velocity, which follows a gaussian distribution with mean 0 and standard deviation stdv, wd denotes the rate of change of angle, which follows a gaussian distribution with mean 0 and standard deviation stdd, wv and wd are the process noise in the particle filter system; t represents the time interval between the k moment and the k-1 moment; the prediction formula gives a mode of obtaining the state of the particle at the moment k from the state of the particle at the moment k-1, namely firstly randomly generating wv and wd, then calculating vk and dk, and calculating xk and yk on the basis;

wk＝1/(sqrt(2*pi)*stdo)*exp(-((xk-x)^2+(yk-y)^2)/(2*(stdo^2)))，

wherein wk represents the weight of the particles at the moment k, pi represents the circumferential rate, stdo is a standard deviation parameter, and exp (t) represents the t-th power of a natural constant e;

wk_(i)＝wk(i)/(wk(1)+wk(2)+…+wk(N))，

wherein wk _ (i) represents the normalized weight of the ith particle at the time k, wk (i) represents the weight of the ith particle at the time k, and 1< ═ i < ═ N, the finally output target coordinate is:

x_＝wk_(1)*xk(1)+wk_(2)*xk(2)+…+wk_(N)*xk(N)，

y_＝wk_(1)*yk(1)+wk_(2)*yk(2)+…+wk_(N)*yk(N)，

wherein x _ represents a target abscissa of final output, y _ represents a target ordinate of final output, (x _, y _) being the target coordinate of final output at time k, xk (i) representing an abscissa of the ith particle at time k, yk (i) representing an ordinate of the ith particle at time k;

8. The method as claimed in claim 7, further comprising step 6, wherein the TDOA vector at the current time is processed, and if there is more data to be input, the method returns to step 1.