CN110308419A - A kind of robust TDOA localization method based on static solution and particle filter - Google Patents
A kind of robust TDOA localization method based on static solution and particle filter Download PDFInfo
- Publication number
- CN110308419A CN110308419A CN201910565034.8A CN201910565034A CN110308419A CN 110308419 A CN110308419 A CN 110308419A CN 201910565034 A CN201910565034 A CN 201910565034A CN 110308419 A CN110308419 A CN 110308419A
- Authority
- CN
- China
- Prior art keywords
- target
- moment
- particle
- base station
- coordinate
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 239000002245 particle Substances 0.000 title claims abstract description 78
- 238000000034 method Methods 0.000 title claims abstract description 38
- 230000003068 static effect Effects 0.000 title claims abstract description 10
- 230000004807 localization Effects 0.000 title claims abstract description 7
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 51
- 230000033001 locomotion Effects 0.000 claims abstract description 50
- 230000005856 abnormality Effects 0.000 claims abstract description 7
- 238000001514 detection method Methods 0.000 claims abstract description 7
- 238000012952 Resampling Methods 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 239000000203 mixture Substances 0.000 claims description 2
- 238000010606 normalization Methods 0.000 claims description 2
- 238000001914 filtration Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 235000013399 edible fruits Nutrition 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 241001269238 Data Species 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000010924 continuous production Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000011514 reflex Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/0009—Transmission of position information to remote stations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The invention discloses a kind of based on the static robust TDOA localization method solved with particle filter, comprising: step 1, reads TDOA vector；Step 2, estimated according to the historical information coordinate current to target, movement velocity, the direction of motion；Step 3, TDOA vector is carried out abnormality detection using state estimation, suppressing exception value；Step 4, if remaining base station number is less than 3, it regard coordinate estimation as target changing coordinates, if remaining base station number is equal to 3, target changing coordinates are solved by simultaneous equations, if remaining base station number is greater than 3, the initial value by coordinate estimation as Taylor algorithm solves target changing coordinates；Step 5, the coordinate input-bound particle filter algorithm that will be obtained, obtains final positioning result；Step 6, which finishes, if there are also data are to be entered, return step 1.
Description
Technical field
The present invention relates to indoor positioning technologies more particularly to a kind of robust TDOA based on static solution and particle filter are fixed
Position method.
Background technique
With the continuous growth of radio network technique to flourish with Internet of Things demand, location based service is just obtained
Various circles of society more and more pay close attention to and study, and it is each that they are widely used in target monitoring, warehouse logistics, business intelligence etc.
Field brings great convenience to life production.Traditional location technology, for example, the U.S. GPS, Europe Galileo and in
The Beidou satellite navigation system of state is achieved in outdoor scene with its wide coverage, the feature that precision is higher, low in cost
Huge success.But indoors in scene, since satellite-signal can not penetrate building, along with there are a large amount of barriers for interior
Block and reflect, the applicability of above-mentioned location technology receives great challenge.In order to solve this problem, researchers mention
Indoor positioning is carried out using equipment such as infrared, bluetooth, ultra wide band, radio frequency identifications out, and is proposed respectively according to the principle used
Class location algorithm, wherein digital method, i.e. TDOA (Time Differenceof Arrival) are a kind of common fixed
Position principle.
According to whether the motion state of target is taken into account, TDOA location algorithm can be mainly divided into two classes: static state is calculated
Method and dynamic algorithm, the ranging information that current time is used only in the former calculates target position, and the latter then can be using target in mistake
Go the status information at moment.A kind of common static state TDOA location algorithm is Taylor algorithm, its basic thought is at some
Initial coordinate nearby carries out Taylor expansion and ignores high order component, then by calculating the local least square method solution of error come excellent
Change the coordinate, and be made iteratively above-mentioned optimization process until coordinate convergence, exports final positioning result.It is connect in initial coordinate
Under nearly true coordinate and the lesser situation of data noise, Taylor algorithm is available more accurate as a result, but in reality
In, the selection of initial value is more difficult, and noise is usually larger in true environment, brings to Taylor algorithm biggish
Error.Document: Foy W H.Position-Location Solutions by Taylor-Series Estimation [J]
.IEEE Transactions on Aerospace&Electronic Systems,2007,AES-12(2):187-194.
Dynamic TDOA location algorithm refers generally to all kinds of filtering algorithms, mainly includes Extended Kalman filter, lossless Kalman
Filtering and particle filter, this kind of algorithm can model noise, and according to the state of target last time and current survey
Alignment by union is carried out away from information, therefore their performances in the biggish environment of noise are generally preferred over static method.Fredrik
Gunnarsson proposes a kind of classical TDOA location algorithm based on particle filter, its basic thought is directly according to reception
To TDOA vector calculate the weight of each particle, and be iteratively performed sampling importance resampling methods four steps, until
Output state convergence, obtains final positioning result.Although the algorithm has certain robustness to noise, under true environment,
The presence of all kinds of Complex Noises can still significantly reduce its positioning accuracy.Document: Gustafsson F, Gunnarsson
F.Positioning using time-difference of arrival measurements[C]//2003IEEE
International Conference on Acoustics,Speech,and Signal Processing,
2003.Proceedings.(ICASSP'03).IEEE,2003,6:VI-553.
Summary of the invention
Goal of the invention: solve tradition TDOA location algorithm face true environment in each noise like when precision it is serious under
Drop, even positioning failure the problem of, propose that abnormality processing strategy promotes the quality of data, and by state algorithm and dynamic algorithm knot
It closes, position error is greatly reduced by way of hierarchical solving, promote track smoothness.
In order to solve the above-mentioned technical problem, the invention discloses a kind of based on the static robust TDOA solved with particle filter
Localization method, this method can be used in the application such as indoor navigation, target monitoring, industrial robot, specifically comprise the following steps:
Step 1, a TDOA vector is read；
Step 2, according to the positioning result of last time, to the coordinate at target current time, movement velocity, the direction of motion into
Row estimation；
Step 3, using the coordinate at the target current time estimated in step 2, movement velocity, the direction of motion to TDOA
Every one-dimensional data of vector carries out abnormality detection, and deletes exceptional value therein；
Step 4, target changing coordinates are solved；
Step 5, by target changing coordinates input-bound particle filter algorithm, final positioning result is obtained.
In step 1, the TDOA vector of reading be relevant to target current time present position TDOA data composition to
Amount, the TDOA vector of reading are expressed as input:
Input=(a21, a31 ..., am1),
Wherein, the difference at a distance from a distance from ai1 indicates target current location between i-th of base station and between the 1st base station,
I indicates the number from base station, and 2≤i≤m, m indicate base station number, and the 1st base station is master base station.
In step 2, has the positioning result of 3 moment t-1, t-2, t-3 in the past, by the distance between them divided by the time difference
The movement velocity of these moment targets is calculated, the movement velocity of setting t-1, t-2, t-3 moment target is respectively v (t-1), v
(t-2), v (t-3), using the direction of t-3 moment and its last moment, that is, t-4 moment coordinate line as t-3 moment target
The direction of motion similarly, will using the direction of t-2 moment and the coordinate line at t-3 moment as the direction of motion of t-2 moment target
The direction of motion of the direction of the coordinate line at t-1 moment and t-2 moment as t-1 moment target, sets t-1, t-2, t-3 moment
The direction of motion of target is respectively d (t-1), d (t-2), d (t-3), using the movement speed at following formula estimation target current time
Degree and direction:
V=(v (t-1)+v (t-2)+v (t-3))/3,
D=(d (t-1)+d (t-2)+d (t-3))/3,
Wherein, v indicates the movement velocity estimation of current target, and d indicates the direction of motion estimation of current target.
The time interval between current time and last moment is set as T, the move distance that can obtain target is △ s=v*T.In setting for the moment
The coordinate for carving target is (x ', y '), then the coordinate estimation of current target are as follows:
Xp=x '+△ s*cos (d),
Yp=y '+△ s*sin (d),
Wherein, xp indicates the abscissa estimation of current target, and yp indicates the ordinate estimation of current target,
Cos (d) expression seeks cosine value to direction d, and sin (d) expression seeks sine value to direction d, (xp, yp) the i.e. seat of current target
Mark estimation.
In step 3, last moment TDOA data relevant to i-th of base station are set as ai1 ', 2≤i≤m is then abnormal
The formula of detection are as follows:
| ai1-ai1 ' | < 2 △ s,
Wherein | ai1-ai1 ' | indicate the absolute value of ai1-ai1 ', above-mentioned formula gives the every one-dimensional of current TDOA data
The upper limit of variable quantity compared with last moment, for being unsatisfactory for the TDOA data of the formula, as exceptional value from TDOA to
It is deleted in amount.
Step 4 includes: for the TDOA vector after suppressing exception value, if remaining base station number less than 3, will obtain in step 2
The coordinate estimation of the current target arrived is used as target changing coordinates, if remaining base station number is equal to 3, is asked by simultaneous equations
Target changing coordinates are solved, if remaining base station number is greater than 3, by the coordinate estimation of current target as the first of Taylor algorithm
Initial value solves target changing coordinates.
In step 4, since step 3 may delete several exceptional values from the TDOA vector received, that is, delete
TDOA data relevant to several base stations need to do different processing for different remaining base station numbers at this time.For deleting
TDOA vector after exceptional value, if remaining base station number is less than 3, i.e., the dimension of the TDOA vector after suppressing exception value is 1 or is
Sky can not be solved at this time, and in order to avoid losing the position at target current time, coordinate obtained in step 2 is estimated
(xp, yp) is used as target changing coordinates.If remaining base station number is equal to 3, i.e. the dimension of TDOA vector after suppressing exception value is
2, set the number of remaining base station as u and v, i.e. this 2 dimension data is respectively au1 and av1,2≤u≤m, 2≤v≤m, at this time
Equation relevant 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) indicates that the coordinate of base station u, (xsv, ysv) indicate that the coordinate of base station v, (xs1, ys1) indicate
The coordinate of master base station, (x, y) indicate that target changing coordinates to be asked, sqrt indicate evolution, and ^2 expression square is above-mentioned by simultaneous
Equation can acquire target changing coordinates.
If remaining base station number is greater than 3, i.e. the dimension of TDOA vector after suppressing exception value is greater than 2, utilizes at this time
Taylor algorithm solves, and since the algorithm needs an accurate initial value to be iterated, coordinate estimation (xp, yp) is made
Initial value thus, to acquire target changing coordinates.
In step 5, confined-particle filtering algorithm is a kind of particle filter based on sampling importance resampling methods design
Algorithm, basic thought are a large amount of particles of simulation, and each particle has a state and a weight, the state distribution of all particles and
Weight simulates the probability distribution of target actual position.The algorithm is to input with coordinate obtained in step 4, after output adjustment
Final coordinate, process include prediction, update, output and this four root phases of resampling, are specifically included:
Step 5-1, forecast period: setting k moment particle state be expressed as (xk, yk, vk, dk), wherein xk, yk, vk,
Dk respectively indicates abscissa, ordinate, movement velocity and the direction of motion of k moment particle, then predictor formula are as follows:
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), d (k-1) respectively indicate the abscissa, ordinate, fortune of k-1 moment particle
Dynamic speed and the direction of motion；Wv indicate speed change rate, obey mean value be 0, the Gaussian Profile that standard deviation is stdv, wd table
Show the change rate of angle, obey mean value be 0, the Gaussian Profile that standard deviation is stdd, wv and wd can be understood as particle filter
Process noise in system；T indicates the time interval between k moment and k-1 moment.Above-mentioned predictor formula gave by the k-1 moment
The state of particle obtains the mode of the state of k moment particle, that is, is first randomly generated wv and wd, then calculating vk and dk, and
Xk and yk is calculated on the basis of this.Binding character in order to increase algorithm was predicted with meeting the particular demands under some special scenes
It joined weight predicting strategy in journey, specific weight predicting strategy is related with the constraint condition of system, for example, working as system requirements target
When speed is no more than 1m/s, weight predicting strategy is to first determine whether it is greater than upper limit 1m/s, such as after calculating particle rapidity vk
Fruit is, then it is assumed that it destroys velocity restraint condition, regenerates vk, is less than 1m/s until it meets, calculates dk, xk again later
And yk；When system requirements target moves in some closed region, for example, a circular stadiums, weight predicting strategy
To check whether it is in the circle, if having exceeded round boundary after calculating particle coordinate (xk, yk), then it is assumed that it is broken
It is broken range constraint condition, regenerate wv, wd and calculates vk, dk, xk and yk, until the coordinate is located in circle.
Step 5-2, more new stage: the target changing coordinates according to obtained in step 4 calculate the weight of each particle, setting
Target changing coordinates are (x, y), then calculation formula are as follows:
Wk=1/ (sqrt (2*pi) * stdo) * exp (- ((xk-x) ^2+ (yk-y) ^2)/(2* (stdo^2))),
Wherein wk indicates the weight of k moment particle, and pi indicates that pi, stdo are standard deviation criteria, and exp (t) is indicated certainly
The t power of right constant e.
Step 5-3, output stage: the prediction and more new stage need to be carried out particle all in system, set
Population is N, after the weight for obtaining all particles, they is normalized, formula are as follows:
Wk_ (i)=wk (i)/(wk (1)+wk (2)+...+wk (N)),
Wherein wk_ (i) indicates the weight after i-th of the particle normalization of k moment, and wk (i) indicates i-th of particle of k moment
Weight, 1≤i≤N.The then coordinates of targets of final output are as follows:
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_ indicates that the target lateral coordinates of final output, y_ indicate that the target ordinate of final output, (x_, y_) are calculated
Coordinates of targets of the method in k moment final output, the abscissa of xk (i) expression i-th of particle of k moment, yk (i) indicate the k moment i-th
The ordinate of a particle, 1≤i≤N.
The resampling stage: step 5-4 carries out importance resampling to particle, regenerates N number of new particle, each grain
The probability that son is resampled is equal to its weight, when solving the positioning result of subsequent time, carries out on new particle.
The invention also includes step 6, the TDOA vector at current time has been disposed, if there are also data are to be entered,
Then return step 1.
The utility model has the advantages that remarkable advantage of the invention is especially to contain a large amount of barriers in all kinds of indoor scenes, block
With reflex in more serious scene, a possibility that positioning fails can reduce, effectively promotion positioning accuracy, obtain target
Stable, smooth motion profile.
Detailed description of the invention
The present invention is done with reference to the accompanying drawings and detailed description and is further illustrated, it is of the invention above-mentioned or
Otherwise advantage will become apparent.
Fig. 1 is overall flow figure of the invention.
Fig. 2 is the flow chart of confined-particle filtering algorithm in the present invention.
Fig. 3 a is that the present invention and several classical ways obtain the comparison diagram of track when holding label in real scene.
Fig. 3 b is that the present invention and several classical ways obtain the comparison diagram of track when wearing label in real scene.
Specific embodiment
The present invention will be further described with reference to the accompanying drawings and embodiments.
Fig. 1 is overall flow figure of the invention, including 6 steps.
In step 1, a TDOA vector is read in, the dimension of the vector is identical as the quantity in positioning system from base station, if
Surely m base station is shared, wherein No. 1 base station is master base station, the base station 2~m is from base station, then the form for reading in TDOA vector is
Input=(a21, a31 ..., am1), wherein ai1 indicate target current location between i-th of base station at a distance from and and master base station
Between distance difference, i indicates from the number of base station, 2≤i≤m.
In step 2, the state at target current time is estimated, which includes the movement velocity of target, movement side
To and coordinate, the step for purpose be to provide support for the data de-noising of subsequent step 3 and the solution procedure of step 4.
Since positioning is a continuous process, has the positioning result of last time at this time, according to these results first to current
Movement velocity and direction are estimated that the estimation mode of speed is averaged to the speed at several moment in past, and direction is estimated
Meter mode is averaged to the direction at several moment in past.Here the specific value of " several " indicates the size of time window,
Estimate the historical information that present speed and when direction are utilized number, value is bigger, and historical information consideration is got in expression
It is more, it is stronger to the robustness of noise, but accuracy is lower, and value is smaller, and expression considers historical information fewer, the knot of estimation
Fruit but easier is influenced closer to the true value at current time by abnormal data.In practical application, window size is generally taken
Value is 3, at this point, the speed at setting t-1, t-2, t-3 moment is respectively v (t-1), v (t-2), v (t-3), direction is respectively d (t-
1), d (t-2), d (t-3), then estimate the movement velocity at target current time and the formula in direction are as follows:
V=(v (t-1)+v (t-2)+v (t-3))/3,
D=(d (t-1)+d (t-2)+d (t-3))/3,
Wherein v indicates the movement velocity estimation of current target, and d indicates the direction of motion estimation of current target.
The time interval between current time and last moment is set as T, the coordinate of last moment target is (x ', y '), obtain it is above-mentioned
After speed and direction estimation, move distance △ s=v*T can be obtained multiplied by the time by speed, to obtain current target
Abscissa estimates xp=x '+△ s*cos (d) and ordinate estimation yp=y '+△ s*sin (d), and wherein cos (d) is indicated to direction
D seeks cosine value, and sin (d) expression seeks sine value to direction d.
In step 3, the TDOA vector denoising to input is needed, this is because due to the presence of ambient noise, input data
In there may be certain exceptional values, if not doing denoising, subsequent coordinate solution procedure will receive serious influence, fixed
Position precision is substantially reduced, in some instances it may even be possible to solve failure.Last moment TDOA data relevant to i-th of base station are set as ai1 ', 2
≤ i≤m, then the formula of abnormality detection be | ai1-ai1 ' | < 2 △ s, wherein | ai1-ai1 ' | indicate ai1-ai1 ' it is absolute
Value.The formula is derived by the triangular relationship between target trajectory and base station location, it give current TDOA to
The upper limit per the one-dimensional variable quantity compared with last moment of amount, is unsatisfactory for those the TDOA data of the formula, as different
Constant value is deleted from vector.It should be noted that the TDOA vector of last moment is not to directly receive here, this be because
It may make to receive between data and its true value there are biggish error for ambient noise, reasonable TDOA vector should be by upper one
The positioning result of moment algorithm final output is back-calculated to obtain with each base station location.
In step 4, since several dimension datas may be deleted during the denoising of last step, TDOA vector at this time
Dimension is uncertain.When its dimension is less than 2, i.e. an only remaining base station or when not having station spare, information content is very few, can not
Coordinate is solved, in order to avoid output of coming to nothing, the estimation of coordinate obtained in step 2 is regard as positioning result, although it is not
It is completely accurate, but it is possible to prevente effectively from positions the case where failing.It is traditional based on most when remaining base station number is 2
The small two TDOA localization methods multiplied can not be applicable in, and can only be solved by most basic simultaneous equations.When remaining base station number
When greater than 2, information content at this time is relatively sufficient, is solved using classical Taylor algorithm, which needs one and align
True initial value completes iteration, and the estimation of coordinate obtained in step 2 can be guaranteed as this initial value in most feelings
Algorithm can restrain under condition, when can not restrain, regard coordinate estimation as positioning result.
In step 5, positioning result obtained in last step, smooth motion rail are adjusted by confined-particle filtering algorithm
Mark exports final positioning result.Confined-particle filtering algorithm joined Particle confinement item on classical particle filter algorithm
Part, to meet the particular demands under special scenes, for example, the vehicle to transport goods is that have in speed in some logistics warehouses
Limit, positioning result has to comply with this requirement.It is false in view of the output of particle filter is the weighted sum of all particle states
Constraint such as is added to each particle, then final output centainly also meets this constraint condition.The flow chart of the algorithm
As shown in Fig. 2, it includes prediction, update, output and this four root phases of resampling.In forecast period, for each grain
Son is first randomly generated the change rate in its movement velocity and direction, i.e. acceleration and angular acceleration, they obey mean value respectively and are
0, standard deviation is the Gaussian Profile of stdv and stdd, and stdv and stdd are two parameters, are usually set to really add close to target
The value of speed and angular acceleration, closer to true value particle state restrain it is faster, if setting deviation it is larger, system it is initial
Stage has certain fluctuation, but with the progress of time, particle state can still restrain.Then calculate particle current time
Speed, direction and coordinate, and check whether and meet constraint condition, it is predicted again if being unsatisfactory for, that is, regenerates acceleration
And angular acceleration.
In the more new stage, the weight of each particle is calculated by the positioning result of the last step inputted, basic thought is grain
Son is closer apart from the result, and the probability that it is in target actual position is higher, and the weight of particle is equal to this probability, obeys equal
Value is the Gaussian Profile that 0, standard deviation is stdo, and stdo is a parameter, it determines the smoothness and delay degree of track,
Stdo is bigger, and motion profile is more smooth, but delay is bigger, i.e., the positioning result that each moment obtains more lags behind its true position
It sets, stdo is smaller, and the smoothness of motion profile is poorer, but postpones smaller.The setting of the parameter depends on specific application scenarios,
In the biggish situation of ambient noise in the scene, it is proposed that otherwise one biggish value of setting suggests being set as 0.1m.
In output stage, the weight of all particles is normalized, and weighted sum exports end-state.
In the resampling stage, N number of particle is regenerated, the higher particle of weight, removal weight are lower in reservation system
Particle, the probability that each particle is resampled are equal to its weight.Population N determines that, to probability density function fitting degree, N is got over
It is fitted better greatly, algorithm effect is better, but calculation amount is bigger.In general, N is set as between 1000 to 5000 to protect
While demonstrate,proving algorithm effect, guarantee reasonable time overhead.
In step 6, the TDOA data at current time have been disposed, and obtained result is for showing motion profile, such as
That there are also data is to be entered for fruit, then return step 1.
Embodiment
In order to verify the validity of proposition method, UWB positioning system is deployed in closed room, and propose to the present invention
Method (the Our method i.e. in Fig. 3 a and Fig. 3 b) and 3 kinds of classic algorithms have carried out contrast test, this 3 kinds of methods include
Taylor algorithm, 2WLS algorithm and particle filter algorithm PF (Taylor, 2WLS, PF i.e. in Fig. 3 a and Fig. 3 b).Room it is big
Small is 6m*6m, wherein have 4 pillars, more than one piece furniture and two blocks of biggish glass, with this come simulate block, reflect, Qiang Gaosi makes an uproar
The critical conditions such as sound, 4 base stations are deployed in 4 corners in room.Tester holds and wears respectively label and goes in a room
It walks, collects two groups of true TDOA data.Data sequence will be collected as test data to calculate in the present invention,
In each step realization and parameter detail it is as follows:
Step 1, TDOA vector is read；
Step 2, estimated according to the historical information coordinate current to target, movement velocity, the direction of motion, estimating speed
3 are sized to time window when direction；
Step 3, TDOA vector is carried out abnormality detection using state estimation, suppressing exception value；
Step 4, target changing coordinates are solved according to different remaining base station numbers；
Step 5, the coordinate input-bound particle filter algorithm that will be obtained, obtains final positioning result, grain in the step
Subnumber N is set as 5000, and standard deviation criteria stdv, stdd, stdo are set to 0.16m/ (s^2), 8rad/ (s^2) and 0.1m；
Step 6, which finishes, if there are also data are to be entered, return step 1.
Collected TDOA data sequence is calculated with 3 kinds of classical ways through the invention when being hand-held label shown in Fig. 3 a
The comparison diagram of track out is that collected TDOA data sequence is passed through with 3 kinds through the invention when wearing label shown in Fig. 3 b
The comparison diagram of the calculated track of allusion quotation method.In the scene of hand-held label, the upper section of real trace close to straight line y=8,
Right-hand part is close to straight line x=4, it can be seen that 2WLS algorithm is entirely ineffective, particle filter algorithm (the PF algorithm i.e. in figure)
Obtained track serious distortion, the result of Taylor algorithm is better than above two algorithm, but there is apparent shake in track, smoothly
Degree is not as good as the present invention.And in the upper right corner of track, Taylor algorithm does not calculate coordinate for some time, and the present invention obtains
To track then there is good continuity, show that its robustness is stronger.In the scene for wearing label, real trace is helically
Shape, since wireless signal can be blocked by body, the noise of this group of data is bigger, is not difficult to find out from effect contrast figure, Taylor
Algorithm, 2WLS algorithm and particle filter algorithm are entirely ineffective, and the track peace with good stability that the present invention obtains
Slippery.Above-mentioned two groups of tests demonstrate validity of the proposition method in real scene, especially in complex environment, performance
Better than conventional method.
The present invention provides a kind of based on the static robust TDOA localization method solved with particle filter, implements the skill
There are many method and approach of art scheme, the above is only a preferred embodiment of the present invention, it is noted that this technology is led
For the those of ordinary skill in domain, various improvements and modifications may be made without departing from the principle of the present invention, these
Improvements and modifications also should be regarded as protection scope of the present invention.The available prior art of each component part being not known in the present embodiment
It is realized.
Claims (8)
1. a kind of based on the static robust TDOA localization method solved with particle filter, which comprises the steps of:
Step 1, a TDOA vector is read；
Step 2, according to the positioning result of last time, the coordinate at target current time, movement velocity, the direction of motion are estimated
Meter；
Step 3, using the coordinate at the target current time estimated in step 2, movement velocity, the direction of motion to TDOA vector
Every one-dimensional data carry out abnormality detection, delete exceptional value therein；
Step 4, target changing coordinates are solved；
Step 5, by target changing coordinates input-bound particle filter algorithm, final positioning result is obtained.
2. the method according to claim 1, wherein in step 1, when the TDOA vector of reading is current with target
The vector of the relevant TDOA data composition in present position is carved, the TDOA vector of reading is expressed as input:
Input=(a21, a31 ..., am1),
Wherein, the difference at a distance from a distance from ai1 indicates target current location between i-th of base station and between the 1st base station, i table
Show the number from base station, 2≤i≤m, m indicate base station number, and the 1st base station is master base station.
3. according to the method described in claim 2, it is characterized in that, having past 3 moment t-1, t-2, t-3 in step 2
Positioning result is calculated the movement velocity of these moment targets by the distance between them divided by the time difference, sets t-1, t-2, t-3
The movement velocity of moment target is respectively v (t-1), v (t-2), v (t-3), by t-3 moment and its last moment, that is, t-4 moment
The direction of motion of the direction of coordinate line as t-3 moment target, similarly, by the side at t-2 moment and the coordinate line at t-3 moment
To the direction of motion as t-2 moment target, using the direction of t-1 moment and the coordinate line at t-2 moment as t-1 moment target
The direction of motion, setting t-1, t-2, t-3 moment target the direction of motion be respectively d (t-1), d (t-2), d (t-3), using such as
The movement velocity at lower formula estimation target current time and direction:
V=(v (t-1)+v (t-2)+v (t-3))/3,
D=(d (t-1)+d (t-2)+d (t-3))/3,
Wherein, v indicates the movement velocity estimation of current target, and d indicates the direction of motion estimation of current target；Setting
Time interval between current time and last moment is T, and the move distance for obtaining target is △ s=v*T；Set last moment mesh
Target coordinate is (x ', y '), then the coordinate estimation of current target are as follows:
Xp=x '+△ s*cos (d),
Yp=y '+△ s*sin (d),
Wherein, xp indicates the abscissa estimation of current target, and yp indicates the ordinate estimation of current target, cos (d)
It indicates to seek direction d cosine value, sin (d) expression seeks sine value to direction d, and (xp, yp) the i.e. coordinate of current target is estimated
Meter.
4. according to the method described in claim 3, it is characterized in that, setting last moment is related to i-th of base station in step 3
TDOA data be ai1 ', the then formula of abnormality detection are as follows:
| ai1-ai1 ' | < 2 △ s,
Wherein | ai1-ai1 ' | indicate ai1-ai1 ' absolute value, above-mentioned formula give current TDOA data per it is one-dimensional with it is upper
One moment compared the upper limit of variable quantity, for being unsatisfactory for the TDOA data of the formula, as exceptional value from TDOA vector
It deletes.
5. according to the method described in claim 4, it is characterized in that, step 4 include: for the TDOA after suppressing exception value to
Amount, if remaining base station number is used as target changing coordinates less than 3, by the coordinate estimation of current target obtained in step 2,
If remaining base station number is equal to 3, target changing coordinates are solved by simultaneous equations, if remaining base station number is greater than 3, when will be current
The coordinate estimation for carving target solves target changing coordinates as the initial value of Taylor algorithm.
6. according to the method described in claim 5, it is characterized in that, if remaining base station number is equal to 3, that is, being deleted different in step 4
The dimension of TDOA vector after constant value is 2, set the number of residue base station as u and v, i.e. this 2 dimension data is respectively au1 and av1,
2≤u≤m, 2≤v≤m, at this time equation relevant 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) indicates that the coordinate of base station u, (xsv, ysv) indicate that the coordinate of base station v, (xs1, ys1) indicate main base
The coordinate stood, (x, y) indicate that target changing coordinates to be asked, sqrt indicate evolution, and ^2 expression square passes through the above-mentioned side of simultaneous
Journey can acquire target changing coordinates.
7. according to the method described in claim 6, it is characterized in that, step 5 includes:
Step 5-1, forecast period: the state of setting k moment particle is expressed as (xk, yk, vk, dk), and wherein xk, yk, vk, dk points
Not Biao Shi k moment particle abscissa, ordinate, movement velocity and the direction of motion, then predictor formula are as follows:
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), d (k-1) respectively indicate the abscissa, ordinate, movement speed of k-1 moment particle
Degree and the direction of motion；Wv indicate speed change rate, obey mean value be 0, the Gaussian Profile that standard deviation is stdv, wd indicate angle
The change rate of degree, obedience mean value is 0, the Gaussian Profile that standard deviation is stdd, wv and wd are the process in particle filter system
Noise；T indicates the time interval between k moment and k-1 moment；Above-mentioned predictor formula gives to be obtained by the state of k-1 moment particle
To the mode of the state of k moment particle, that is, it is first randomly generated wv and wd, then calculates vk and dk, and calculate on this basis
Xk and yk；
Step 5-2, more new stage: the target changing coordinates according to obtained in step 4 calculate the weight of each particle, set target
Changing coordinates are (x, y), then calculation formula are as follows:
Wk=1/ (sqrt (2*pi) * stdo) * exp (- ((xk-x) ^2+ (yk-y) ^2)/(2* (stdo^2))),
Wherein wk indicates the weight of k moment particle, and pi indicates that pi, stdo are standard deviation criteria, and exp (t) indicates that nature is normal
The t power of number e；
Step 5-3, output stage: the prediction and more new stage need to be carried out particle all in system, set particle
Number is that N normalizes them after the weight for obtaining all particles, formula are as follows:
Wk_ (i)=wk (i)/(wk (1)+wk (2)+...+wk (N)),
Wherein wk_ (i) indicates the weight after i-th of the particle normalization of k moment, and wk (i) indicates the weight of i-th of particle of k moment,
1≤i≤N, the then coordinates of targets of final output are as follows:
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_ indicates that the target lateral coordinates of final output, y_ indicate the target ordinate of final output, and (x_, y_) is i.e. in k
The coordinates of targets of final output is carved, xk (i) indicates that the abscissa of i-th of particle of k moment, yk (i) indicate i-th of particle of k moment
Ordinate.
The resampling stage: step 5-4 carries out importance resampling to particle, regenerates N number of new particle, each particle quilt
The probability of resampling is equal to its weight, when solving the positioning result of subsequent time, carries out on new particle.
8. the TDOA vector at current time has been located the method according to the description of claim 7 is characterized in that further including step 6
Reason finishes, if there are also data are to be entered, return step 1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910565034.8A CN110308419B (en) | 2019-06-27 | 2019-06-27 | Robust TDOA (time difference of arrival) positioning method based on static solution and particle filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910565034.8A CN110308419B (en) | 2019-06-27 | 2019-06-27 | Robust TDOA (time difference of arrival) positioning method based on static solution and particle filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110308419A true CN110308419A (en) | 2019-10-08 |
| CN110308419B CN110308419B (en) | 2021-04-06 |
Family
ID=68076270
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910565034.8A Active CN110308419B (en) | 2019-06-27 | 2019-06-27 | Robust TDOA (time difference of arrival) positioning method based on static solution and particle filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110308419B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112066981A (en) * | 2020-09-07 | 2020-12-11 | 南京大学 | Three-dimensional positioning tracking method in complex environment |
| CN115508774A (en) * | 2022-10-12 | 2022-12-23 | 中国电子科技集团公司信息科学研究院 | Time difference positioning method and device based on two-step weighted least square and storage medium |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2570772A1 (en) * | 2011-09-16 | 2013-03-20 | Deutsches Zentrum für Luft- und Raumfahrt e.V. | Method for localisation and mapping of pedestrians or robots using wireless access points |
| CN105898865A (en) * | 2016-06-17 | 2016-08-24 | 杭州电子科技大学 | Cooperative location method based on EKF (Extended Kalman Filter) and PF (Particle Filter) under nonlinear and non-Gaussian condition |
| CN107680120A (en) * | 2017-09-05 | 2018-02-09 | 南京理工大学 | Tracking Method of IR Small Target based on rarefaction representation and transfer confined-particle filtering |
| CN108151747A (en) * | 2017-12-27 | 2018-06-12 | 浙江大学 | A kind of indoor locating system and localization method merged using acoustical signal with inertial navigation |
-
2019
- 2019-06-27 CN CN201910565034.8A patent/CN110308419B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2570772A1 (en) * | 2011-09-16 | 2013-03-20 | Deutsches Zentrum für Luft- und Raumfahrt e.V. | Method for localisation and mapping of pedestrians or robots using wireless access points |
| CN105898865A (en) * | 2016-06-17 | 2016-08-24 | 杭州电子科技大学 | Cooperative location method based on EKF (Extended Kalman Filter) and PF (Particle Filter) under nonlinear and non-Gaussian condition |
| CN107680120A (en) * | 2017-09-05 | 2018-02-09 | 南京理工大学 | Tracking Method of IR Small Target based on rarefaction representation and transfer confined-particle filtering |
| CN108151747A (en) * | 2017-12-27 | 2018-06-12 | 浙江大学 | A kind of indoor locating system and localization method merged using acoustical signal with inertial navigation |
Non-Patent Citations (2)
| Title |
|---|
| XU B ET AL.: ""High-Accuracy TDOA-based Localization without Time Synchronization"", 《IEEE TRANSACTIONS ON PARALLEL & DISTRIBUTED SYSTEM》 * |
| 卞瑞祥: ""一种无线传感器网络的定位精度改进方法"", 《计算机应用与软件》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112066981A (en) * | 2020-09-07 | 2020-12-11 | 南京大学 | Three-dimensional positioning tracking method in complex environment |
| CN112066981B (en) * | 2020-09-07 | 2022-06-07 | 南京大学 | Three-dimensional positioning tracking method in complex environment |
| CN115508774A (en) * | 2022-10-12 | 2022-12-23 | 中国电子科技集团公司信息科学研究院 | Time difference positioning method and device based on two-step weighted least square and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110308419B (en) | 2021-04-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110244715B (en) | Multi-mobile-robot high-precision cooperative tracking method based on ultra wide band technology | |
| CN113074739B (en) | UWB/INS fusion positioning method based on dynamic robust volume Kalman | |
| CN108307301B (en) | Indoor positioning method based on RSSI ranging and track similarity | |
| CN108519587B (en) | Real-time aerial target motion mode identification and parameter estimation method | |
| CN108717174B (en) | Information theory-based passive cooperative positioning method for predicting rapid covariance interaction fusion | |
| CN106908762B (en) | A kind of more hypothesis UKF method for tracking target for UHF-RFID system | |
| Ullah et al. | Evaluation of Localization by Extended Kalman Filter, Unscented Kalman Filter, and Particle Filter‐Based Techniques | |
| CN110308419A (en) | A kind of robust TDOA localization method based on static solution and particle filter | |
| CN110850363B (en) | Method for carrying out dynamic filtering optimization based on real-time positioning track data | |
| CN102088769A (en) | Wireless location method for directly estimating and eliminating non-line-of-sight (NLOS) error | |
| CN114919627B (en) | RIS technology-based train positioning tracking method | |
| CN114615740A (en) | Indoor personnel positioning method based on Bluetooth, PDR and map matching fusion | |
| Li et al. | A novel method of WiFi fingerprint positioning using spatial multi-points matching | |
| CN111982102A (en) | BP-EKF-based UWB-IMU positioning method in complex environment | |
| CN107356932A (en) | Robotic laser localization method | |
| CN115775473A (en) | Aircraft positioning system in ADS-B aviation monitoring system | |
| Lee et al. | TDoA based UAV localization using dual-EKF algorithm | |
| Zhou et al. | A Robust Detection and Optimization Approach for Delayed Measurements in UWB Particle-Filter-Based Indoor Positioning | |
| CN114236480A (en) | Airborne platform sensor system error registration algorithm | |
| CN106353724A (en) | Method for accurate indoor positioning based on wireless sensor network | |
| CN113759392A (en) | Robust GNSS interference source positioning method based on flight big data | |
| CN109709592A (en) | A kind of Beidou auxiliary train location algorithm | |
| CN110728309B (en) | Track clustering method based on railway signals and Beidou positioning | |
| CN112333634A (en) | Hybrid node positioning method based on UAV | |
| Zhang et al. | An adaptive road-constrained IMM estimator for ground target tracking in GSM networks |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |