CN115248415B - Target positioning and tracking method for mine non-line-of-sight scene - Google Patents
Target positioning and tracking method for mine non-line-of-sight scene Download PDFInfo
- Publication number
- CN115248415B CN115248415B CN202210732436.4A CN202210732436A CN115248415B CN 115248415 B CN115248415 B CN 115248415B CN 202210732436 A CN202210732436 A CN 202210732436A CN 115248415 B CN115248415 B CN 115248415B
- Authority
- CN
- China
- Prior art keywords
- line
- sight
- target
- base station
- particles
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 91
- 239000002245 particle Substances 0.000 claims abstract description 121
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 52
- 238000001914 filtration Methods 0.000 claims abstract description 36
- 238000012952 Resampling Methods 0.000 claims abstract description 35
- 230000003068 static effect Effects 0.000 claims abstract description 24
- 238000012937 correction Methods 0.000 claims abstract description 23
- 238000005259 measurement Methods 0.000 claims abstract description 15
- 230000006854 communication Effects 0.000 claims description 15
- 238000004891 communication Methods 0.000 claims description 12
- 230000007704 transition Effects 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 5
- 238000009795 derivation Methods 0.000 claims description 3
- 239000011164 primary particle Substances 0.000 claims description 3
- 230000000694 effects Effects 0.000 description 6
- 238000004088 simulation Methods 0.000 description 6
- 230000006978 adaptation Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000002922 simulated annealing Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 239000003245 coal Substances 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005764 inhibitory process Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- 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/0205—Details
- G01S5/021—Calibration, monitoring or correction
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Radar Systems Or Details Thereof (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The invention discloses a target positioning tracking method for a mine non-line-of-sight scene, which comprises the steps of firstly judging a measured value variance to identify whether a non-line-of-sight signal is transmitted or not through a non-line-of-sight error vector and a correction factor vector, and identifying a base station with the non-line-of-sight error; the improved Chan algorithm is adopted to reduce the system measurement error, so that the purpose of improving the positioning accuracy of the static target is achieved; for tracking the track when the target moves, the invention adopts an improved particle filtering algorithm to track the moving target, uses the corrected distance difference value data when the static target is positioned as the observed quantity of particle filtering, and reduces the error of the follow-up tracking track; the concept of effective particle number is introduced, and whether a resampling process is carried out or not is determined by setting a threshold value, so that the operand is reduced, and the operation efficiency is improved; and ChopThin resampling is introduced into a particle filtering algorithm, so that the number of effective particles and the diversity of the particles are ensured, and the influence of the lack of the diversity of the particles on the filtering performance is relieved.
Description
Technical Field
The invention relates to a target positioning and tracking method under a mine, in particular to a target positioning and tracking method for a non-line-of-sight scene of the mine.
Background
The mine moving target positioning tracking system is one of important supporting systems for coal mine production, and can determine the positions and movement tracks of underground personnel and equipment and the positions of trapped personnel in sudden accidents so as to scientifically formulate rescue schemes. In the mine positioning and tracking system, a ranging positioning method mainly adopting technologies such as Time of Arrival (TOA), time difference of Arrival (TIME DIFFERENCE of Arrival, TDOA), angle of Arrival (AOA), received signal strength (RECEIVED SIGNAL STRENGTH Indicator, RSSI) and the like is adopted, wherein the TDOA method is easy to realize, low in computational complexity and high in positioning accuracy, and is one of main methods of mine target positioning and tracking.
Because the mine tunnel environment is complex and changeable, the positioning signal can reach the receiving end only through the reflection of the tunnel wall or even multiple reflections by the obstacles such as personnel, vehicles and equipment, and the positioning accuracy of the mine moving target positioning system is limited because the TDOA measurement value is introduced with non-line-of-sight (Non Line of Sight, NLOS) errors. The Chan algorithm is a classical algorithm for solving a TDOA positioning equation, adopts two-step weighted least square to realize the position calculation of a target node, has the characteristic of high operation speed, can realize higher positioning precision under the condition of Line of Sight (LOS), but can drastically reduce the performance of the Chan algorithm in a non-Line of Sight environment. In addition, since the position of the moving target at the current time has a correlation with the position at the previous time, if the target position with the non-line-of-sight error at the previous time is still used for positioning at the next time, larger error accumulation is likely to be brought.
In addition, after the target is positioned, for example, the target is continuously moved, and the tracking and positioning process is a typical filtering problem, and currently widely used filtering algorithms include Kalman filtering (KALMAN FILTER, KF), extended Kalman filtering (Extended KALMAN FILTER, EKF), unscented Kalman filtering (Unscented KALMAN FILTER, UKF) and particle filtering (PARTICLE FILTER, PF) algorithms. KF is suitable for linear gaussian systems, and EKF and UKF, although solving the problem of target state estimation under certain forms of nonlinear, non-gaussian conditions, have strong limitations on the system model. The PF has no limitation on the system, has better estimation performance on a nonlinear non-Gaussian system, and is more suitable for tracking a moving target in a complex mine environment. However, the problem of particle depletion caused by the resampling process of the traditional PF algorithm results in poor filtering performance, so how to provide a method for effectively reducing the non-line-of-sight error when the target in the non-line-of-sight environment in the mine is stationary positioned, thereby improving the positioning precision of the target, and effectively improving the tracking precision of the moving track when the target moves subsequently, is one of the research directions in the industry.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a target positioning and tracking method for a mine non-line-of-sight scene, which can effectively reduce non-line-of-sight errors when a target in the mine non-line-of-sight environment is subjected to static positioning, thereby improving the positioning precision of the target and effectively improving the tracking precision of a moving track when the target moves subsequently.
In order to achieve the above purpose, the technical scheme adopted by the invention is as follows: a target positioning and tracking method for mine non-line-of-sight scenes comprises the following specific steps:
step one, determining a positioning tracking range and the number of base stations: firstly, determining a range area which is needed to be subjected to target positioning and tracking under a mine, and determining the number of base stations in the range area according to the range area;
Step two, determining a static target positioning model in a sight distance environment and a non-sight distance environment: setting the number of the base stations as M, setting the coordinates as z i＝[xi yi]T, i=1, 2, … and M, and numbering the base stations in sequence; the coordinate of the target to be measured is z= [ x y ] T; taking the base station with the number of 1 as a reference station, and respectively establishing a distance difference equation set from a target to other base stations i and the reference base station in a line-of-sight environment and a non-line-of-sight environment;
Judging whether non-line-of-sight errors exist in the communication positioning process of each base station or not: let the measurement error e i,1 obey the mean value 0 and the variance 0 Gaussian distribution, mean of non-line-of-sight error n i,1 of u ni,1, variance of/>Its standard deviation is σ ni,1; setting a base station vector A= [ A 1 A2 … Ai ] with non-line-of-sight errors, wherein A i epsilon [0,1]; if a i =1, determining the ith base station as the base station with the non-line-of-sight error, otherwise, the base station does not have the non-line-of-sight error; setting a correction factor vector alpha= [ alpha 1 α2 … αi ], setting a non-line-of-sight error (namely NLOS error) received by an ith base station as alpha iμni,1, giving an initial value to the correction factor vector alpha i in the first judgment, and obtaining the value of the correction factor vector alpha i in the subsequent judgment by updating and solving; when alpha iσni,1 is more than or equal to 0.5, at the moment, A i =1, and the ith base station is the base station with non-line-of-sight error; according to the step, whether the non-line-of-sight errors exist in the positioning communication process of each base station can be judged;
fourthly, static target positioning is carried out in the view distance scene: according to the judging result of the step three, if no non-line-of-sight error exists in the communication positioning process of each base station, solving a distance difference equation set from the target to other base stations i and the reference base station in the line-of-sight environment obtained in the step two by adopting a Chan algorithm, thereby obtaining the target position in the line-of-sight scene, completing the static target positioning process and entering the step six; if the non-line-of-sight errors exist in the communication positioning process of each base station, entering a step five;
Fifthly, static target positioning is carried out in a non-line-of-sight scene: according to the judging result of the step three, if the non-line-of-sight errors exist in the communication positioning process of each base station, correcting a distance difference equation set from the target to other base stations i and the reference base station in the non-line-of-sight environment obtained in the step two through a non-line-of-sight error mean value u ni,1 and a base station vector A i of the non-line-of-sight errors, then solving the corrected distance difference equation set by adopting a Chan algorithm, thereby obtaining the target position in the non-line-of-sight scene, completing the static target positioning process and entering the step six;
Step six, tracking the subsequent moving process of the target: the improved particle filtering algorithm is adopted to estimate the state in the moving process of the target, and the method specifically comprises the following steps:
the improved particle filtering algorithm is based on state and observation equations, i.e
xk＝Fxk-1+Qk-1
zk＝h(xk)+Rk
Wherein F is a state transition matrix, and Q k、Rk is state transition noise and observation noise respectively; taking the corrected distance difference value obtained in the step five as the observed quantity of the improved particle filtering algorithm;
The improved particle filter algorithm comprises the following specific steps:
① Initializing: sampling from the a priori distribution p (x 0) to obtain a set of primary particles at time k=0 Setting particle weight/>Since the particle distribution condition is unknown at the beginning, the prior distribution p (x 0) in the initialization stage can be any distribution, such as average distribution;
② Prediction stage: because of the nonlinearity of the equation, the posterior probability cannot be directly sampled, so an importance distribution q (x k|x0:k-1) convenient for sampling is introduced, namely
③ Updating: when data z is observed, the weight of each particle is updated and normalized:
Wherein, For likelihood distribution,/>Is a state transition distribution;
At this time, the posterior probability density is The new particle group is/>
④ Calculating the effective particle number and setting a threshold value: defining the effective particle number as N eff, setting a threshold value for reducing the operation amount of a particle filtering algorithmIf N eff<Nt is not found, go to step ⑤, otherwise go to step ⑥;
Wherein round is a rounding operation;
⑤ Resampling: carrying out ChopThin resampling to obtain a group of particles with unequal weights;
⑥ Output state estimation: and obtaining the state estimation of each moment of the target according to the obtained data by the following formula:
thereby realizing the process of tracking the moving track when the target moves.
Further, in the second step, the equation set of the distance difference between the target and the other base station i and the reference base station in the range environment is as follows:
The distance difference equation set between the target and other base stations i and the reference base station in the non-line-of-sight environment is as follows:
Where e i,1、ni,1 denotes the measurement error and the non-line-of-sight error, respectively, c is the electromagnetic wave propagation speed, τ i,1 is the arrival time difference between the target position to the other base station i and the reference base station.
Further, the update solution process of the correction factor vector α i in the third step is:
Defining the residual function of the i-th base station distance difference r i,1 as:
the formula carries out derivation on alpha i to lead the derivative to be 0, and update solution of the correction factor alpha i is completed; wherein the method comprises the steps of Updating the correction factor alpha i after the current target position coordinate is determined;
further, the fifth step specifically comprises:
Since the non-line-of-sight error does not follow zero mean distribution, the ranging result is corrected as follows:
and (3) carrying out correction on the distance difference value by the distance difference value equation set under the non-line-of-sight environment obtained in the step (II), wherein the corrected formula is as follows:
Let the z be a＝,x y R]T to be the same as the first, Assuming that the linearity of x, y and R is irrelevant, then:
and (3) finishing to obtain:
Wherein x i,1＝xi-x1, i=1, 2, …, M;
The corrected distance difference value is carried into a Chan algorithm to calculate a target position, and an error vector is defined by considering non-line-of-sight errors and measurement errors:
Wherein,
The error vector has the following statistical features:
Wherein the method comprises the steps of ,B＝diag{[r2 r3 … rM]},e＝[e2,1 e3,1 … eM,1]T,n＝[n2,1 n3,1 … nM,1]T;
The covariance matrix of the noise is:
the WLS algorithm is used for the first time to obtain an initial solution of the target:
is the result obtained assuming that x, y, and R are independent of each other in z a＝[x y R]T, and in reality R is a quantity related to (x, y); in order to obtain a more accurate positioning result, a second WLS estimation is performed by using the initial solution obtained for the first time as a known constraint condition;
Order the The error vector becomes:
and (4) obtaining by using WLS estimation again:
Wherein the method comprises the steps of
The position of the target is finally obtained as follows:
thereby obtaining the position of the stationary object in the non-line-of-sight scene.
Further, the specific resampling process in the sixth step ChopThin is as follows:
defining the weight ratio of particles as eta, the threshold value as a and the number of particle offspring with weight as w as And meet/>Wherein N is the number of particles before resampling, and N is the target number of particles; the specific steps of resampling are:
1) Determining a threshold value a: a determines whether the particles are subjected to Chop (i.e., shredding) or Thin (i.e., thinning) as a threshold value, and the determination method of a is as follows:
Dividing the weight value w into two sets of w l and w u, uniformly distributing and sampling w l or w u to obtain a, and making Calculating the resampled particle number:
Wherein s l is the sum of the particle weights with the weight value greater than or equal to a, s u is the sum of the particle weights with the weight value greater than or equal to b, c m is the number of particles with the weight value greater than or equal to a, and c u is the number of particles with the weight value greater than or equal to b;
If h=n, return a, i.e Otherwise, the element is deleted from w l or w u and the weight sum is updated until h=n is satisfied;
2) The Thin process comprises the following steps: determining new weight and offspring number of small weight (w i < a) particles, wherein the offspring number of the particles is determined by Carrying out system resampling and determining, wherein the weight of offspring particles is set as a, the number of particles processed by Thin is N L, and the total weight of the particles is aN L;
3) Setting a weight adjustment parameter K: the step 2) obtains that the total weight is possibly changed, and K is used for adjusting the total weight before the loop processing to ensure that the total weight is unchanged
Wherein L= { j: w j<a},U＝{j:wj. Gtoreq.a };
4) In the Chop process, large-weight particles (w i. Gtoreq.a) are chopped, each large-weight particle is obtained Offspring, wherein m i is pair/>The fractional part of (a) is re-sampled twice, the particle weight range of the stage is (a, eta a), the total number of returned particles is N m, and N m+NL =N is satisfied.
One important cause of positioning error compared to the prior art is TDOA ranging error due to non-line-of-sight propagation of signals between the target and the base station. To reduce the effect of non-line-of-sight errors on positioning accuracy, base stations with non-line-of-sight errors are first identified, and which base stations have non-line-of-sight errors are determined to correct for the non-line-of-sight errors. According to the invention, a non-line-of-sight error vector and a correction factor vector are introduced, whether non-line-of-sight signal transmission exists or not is identified by judging the variance of the measured value, and then an improved Chan algorithm is adopted to reduce the system measurement error, so that the purpose of improving the positioning accuracy of a static target is achieved; for tracking the track when the target moves, the invention adopts an improved particle filtering algorithm to track the moving target, and compared with the common particle filtering, the invention has three improvements: firstly, taking corrected distance difference data during static target positioning as observed quantity of particle filtering, and reducing errors of follow-up tracking tracks; secondly, the concept of effective particle number is introduced, and whether a resampling process is carried out or not is determined by setting a threshold value, so that the operand is reduced, and the operation efficiency is improved; thirdly, chopThin resampling is introduced into a particle filtering algorithm, a group of particles with unequal weights are generated through a Chop processing stage and a Thin processing stage, the number of effective particles and the diversity of the particles are ensured, and therefore the influence of the diversity shortage of the particles on the filtering performance is relieved.
Drawings
FIG. 1 is an overall flow chart of the present invention;
FIG. 2 is a graph of positioning errors from a positioning simulation test of a stationary target using three different methods;
FIG. 3 is a graph of a tracking trajectory from a tracking simulation of a moving object using a number of different methods;
FIG. 4 is a graph of tracking errors from a tracking simulation of a moving object using a number of different methods.
Detailed Description
The present invention will be further described below.
As shown in fig. 1, the specific steps of the present invention are:
step one, determining a positioning tracking range and the number of base stations: firstly, determining a range area which is needed to be subjected to target positioning and tracking under a mine, and determining the number of base stations in the range area according to the range area;
Step two, determining a static target positioning model in a sight distance environment and a non-sight distance environment: setting the number of the base stations as M, setting the coordinates as z i＝[xi yi]T, i=1, 2, … and M, and numbering the base stations in sequence; the coordinate of the target to be measured is z= [ x y ] T; taking the base station with the number of 1 as a reference station, and respectively establishing a distance difference equation set from a target to other base stations i and the reference base station in a line-of-sight environment and a non-line-of-sight environment; the distance difference equation set between the target and other base stations i and the reference base station in the line-of-sight environment is as follows:
The distance difference equation set between the target and other base stations i and the reference base station in the non-line-of-sight environment is as follows:
Where e i,1、ni,1 denotes the measurement error and the non-line-of-sight error, respectively, c is the electromagnetic wave propagation speed, τ i,1 is the arrival time difference between the target position to the other base station i and the reference base station.
Judging whether non-line-of-sight errors exist in the communication positioning process of each base station or not: let the measurement error e i,1 obey the mean value 0 and the variance 0Gaussian distribution, mean of non-line-of-sight error n i,1 of u ni,1, variance of/>Its standard deviation is σ ni,1; setting a base station vector A= [ A 1 A2 … Ai ] with non-line-of-sight errors, wherein A i epsilon [0,1]; if a i =1, determining the ith base station as the base station with the non-line-of-sight error, otherwise, the base station does not have the non-line-of-sight error; setting a correction factor vector alpha= [ alpha 1 α2 … αi ], wherein the non-line-of-sight error received by an ith base station is alpha iμni,1, giving an initial value to the correction factor vector alpha i in the first judgment, and obtaining the value of the correction factor vector alpha i in the subsequent judgment through updating and solving; when alpha iσni,1 is more than or equal to 0.5, at the moment, A i =1, and the ith base station is the base station with non-line-of-sight error; according to the step, whether the non-line-of-sight errors exist in the positioning communication process of each base station can be judged; the update solution process of the correction factor vector alpha i is as follows:
Defining the residual function of the i-th base station distance difference r i,1 as:
the formula carries out derivation on alpha i to lead the derivative to be 0, and update solution of the correction factor alpha i is completed; wherein the method comprises the steps of Updating the correction factor alpha i after the current target position coordinate is determined;
fourthly, static target positioning is carried out in the view distance scene: according to the judging result of the step three, if no non-line-of-sight error exists in the communication positioning process of each base station, solving a distance difference equation set from the target to other base stations i and the reference base station in the line-of-sight environment obtained in the step two by adopting a Chan algorithm, thereby obtaining the target position in the line-of-sight scene, completing the static target positioning process and entering the step six; if the non-line-of-sight errors exist in the communication positioning process of each base station, entering a step five;
Fifthly, static target positioning is carried out in a non-line-of-sight scene: according to the judging result of the step three, if the non-line-of-sight errors exist in the communication positioning process of each base station, correcting a distance difference equation set from the target to other base stations i and the reference base station in the non-line-of-sight environment obtained in the step two through a non-line-of-sight error mean value u ni,1 and a base station vector A i of the non-line-of-sight errors, then solving the corrected distance difference equation set by adopting a Chan algorithm, thereby obtaining the target position in the non-line-of-sight scene, completing the static target positioning process and entering the step six; the method comprises the following steps:
Since the non-line-of-sight error does not follow zero mean distribution, the ranging result is corrected as follows:
and (3) carrying out correction on the distance difference value by the distance difference value equation set under the non-line-of-sight environment obtained in the step (II), wherein the corrected formula is as follows:
let the z be a＝[x y R]T to be the same as the first, Assuming that the linearity of x, y and R is irrelevant, then:
and (3) finishing to obtain:
Wherein x i,1＝xi-x1, i=1, 2, …, M;
The corrected distance difference value is carried into a Chan algorithm to calculate a target position, and an error vector is defined by considering non-line-of-sight errors and measurement errors:
Wherein,
The error vector has the following statistical features:
Wherein the method comprises the steps of ,B＝diag{[r2 r3 … rM]},e＝[e2,1 e3,1 … eM,1]T,n＝[n2,1 n3,1 … nM,1]T;
The covariance matrix of the noise is:
the WLS algorithm is used for the first time to obtain an initial solution of the target:
is the result obtained assuming that x, y, and R are independent of each other in z a＝[x y R-T, and in reality R is a quantity related to (x, y); in order to obtain a more accurate positioning result, a second WLS estimation is performed by using the initial solution obtained for the first time as a known constraint condition;
Order the The error vector becomes:
and (4) obtaining by using WLS estimation again:
Wherein the method comprises the steps of
The position of the target is finally obtained as follows:
thereby obtaining the position of the stationary object in the non-line-of-sight scene.
Step six, tracking the subsequent moving process of the target: the improved particle filtering algorithm is adopted to estimate the state in the moving process of the target, and the method specifically comprises the following steps:
the improved particle filtering algorithm is based on state and observation equations, i.e
xk＝Fxk-1+Qk-1
zk＝h(xk)+Rk
Wherein F is a state transition matrix, and Q k、Rk is state transition noise and observation noise respectively; taking the corrected distance difference value obtained in the step five as the observed quantity of the improved particle filtering algorithm;
The improved particle filter algorithm comprises the following specific steps:
① Initializing: sampling from the a priori distribution p (x 0) to obtain a set of primary particles at time k=0 Setting particle weight/>Since the particle distribution condition is unknown at the beginning, the prior distribution p (x 0) in the initialization stage can be any distribution, such as average distribution;
② Prediction stage: because of the nonlinearity of the equation, the posterior probability cannot be directly sampled, so an importance distribution q (x k|x0:k-1) convenient for sampling is introduced, namely
③ Updating: when data z is observed, the weight of each particle is updated and normalized:
Wherein, For likelihood distribution,/>Is a state transition distribution;
At this time, the posterior probability density is The new particle group is/>
④ Calculating the effective particle number and setting a threshold value: defining the effective particle number as N eff, setting a threshold value for reducing the operation amount of a particle filtering algorithmIf N eff<Nt is not found, go to step ⑤, otherwise go to step ⑥;
Wherein round is a rounding operation;
⑤ Resampling: adopting ChopThin resampling to obtain a group of particles with unequal weights, wherein the specific process of resampling is as follows:
defining the weight ratio of particles as eta, the threshold value as a and the number of particle offspring with weight as w as And meet/>Wherein N is the number of particles before resampling, and N is the target number of particles; the specific steps of resampling are:
1) Determining a threshold value a: a determines whether the particles are subjected to Chop (i.e., shredding) or Thin (i.e., thinning) as a threshold value, and the determination method of a is as follows:
Dividing the weight value w into two sets of w l and w u, uniformly distributing and sampling w l or w u to obtain a, and making Calculating the resampled particle number:
Wherein s l is the sum of the particle weights with the weight value greater than or equal to a, s u is the sum of the particle weights with the weight value greater than or equal to b, c m is the number of particles with the weight value greater than or equal to a, and c u is the number of particles with the weight value greater than or equal to b;
If h=n, return a, i.e Otherwise, the element is deleted from w l or w u and the weight sum is updated until h=n is satisfied;
2) The Thin process comprises the following steps: determining new weight and offspring number of small weight (w i < a) particles, wherein the offspring number of the particles is determined by Carrying out system resampling and determining, wherein the weight of offspring particles is set as a, the number of particles processed by Thin is N L, and the total weight of the particles is aN L;
3) Setting a weight adjustment parameter K: the step 2) obtains that the total weight is possibly changed, and K is used for adjusting the total weight before the loop processing to ensure that the total weight is unchanged
Wherein L= { j: w j<a},U＝{j:wj. Gtoreq.a };
4) In the Chop process, large-weight particles (w i. Gtoreq.a) are chopped, each large-weight particle is obtained Offspring, wherein m i is pair/>The fractional part of (a) is re-sampled twice, the particle weight range of the stage is (a, eta a), the total number of returned particles is N m, and N m+NL =N is satisfied.
⑥ Output state estimation: and obtaining the state estimation of each moment of the target according to the obtained data by the following formula:
thereby realizing the process of tracking the moving track when the target moves.
And (3) effect verification:
A Matlab is adopted to carry out simulation experiments on the method, so that the positioning and tracking performance of the algorithm is analyzed. The experimental environment is a long straight roadway with the length of 5m and the length of 5m, 5 base stations are deployed on the roadway wall at equal intervals, and the coordinates of the long straight roadway are BS1 (0, 0), BS2 (25, 5), BS3 (50, 0), BS4 (75, 5) and BS5 (100, 0). The positioning accuracy is measured using Root Mean Square Error (RMSE), expressed as:
Wherein, And (x, y) are the target estimated position and the true position, respectively.
And (3) verifying the positioning accuracy of the static target:
The measurement error generally follows an ideal gaussian distribution with little impact on the total error, and the standard deviation of the measurement error is set to 0.2m. Mine NLOS (i.e., non-line-of-sight) errors generally follow an exponential or gaussian distribution, where a gaussian distribution is used, setting the average of the NLOS errors to 15m. Assuming that BS2, BS3 and BS4 have NLOS errors, the number of simulation runs is 1000, and taking the average value of the running results as the positioning result.
In order to verify the positioning performance of the invention on the static target, a simulation experiment is carried out on the mine static target. The improved Chan algorithm with NLOS inhibition of the invention is compared with the classical Chan algorithm and the Chan method optimized based on simulated annealing method, and the result is shown in figure 2. In order to more intuitively compare the positioning effects of 3 positioning methods, it can be seen from the mean square value of fig. 2 that in a mine environment where NLOS propagation is serious, the existing simulated annealing method assists the Chan algorithm to obtain a high-quality target position estimation method, and although the positioning accuracy of the traditional Chan algorithm can be improved, the method does not consider the influence of NLOS errors, so that the accuracy in mine target positioning is not high. Compared with other two methods, the method has better positioning precision for the static target, which shows that the NLOS error suppression method provided by the invention can filter out the measured data polluted by the NLOS and has the capability of improving the positioning precision of the mine target.
Performance verification of moving target tracking its trajectory:
as with ordinary particle filtering, CTPF is also based on the state and observation equations, i.e
xk＝Fxk-1+Qk-1
zk＝h(xk)+Rk
Wherein F is a state transition matrix, and Q k、Rk is state transition noise and observation noise, respectively.
Assuming that the target moves at a uniform speed in the mine tunnel, the state vector at the moment kState transition matrix sampling CV model, i.e./>Process noise covariance is/>The distance measurement value of NLOS error elimination of the invention is used as the observed quantity of the moving target tracking of the invention, wherein the observed noise variance is set as R k =diag ([ 1,1 ]), the sampling period T=1s, and the initial state x 0 = [0,1,3,0] of the target. The number of particles n=300, and the number of monte carlo simulations m=50.
The improved particle filter algorithm (i.e., CTPF algorithm) for moving object tracking of the present invention is compared to conventional resampling particle filter algorithms such as random resampling, polynomial resampling, and systematic resampling. As shown in fig. 3 and 4, it can be seen that all the particle filtering algorithms can stably track the target, no filter divergence occurs, and the tracking effect of random resampling is the worst, and the fluctuation degree of the tracking error is the highest. The tracking precision of the polynomial resampling and the system resampling is similar, and the polynomial resampling and the system resampling have better tracking effect in comparison. The tracking precision of the CTPF particle filtering algorithm based on ChopThin resampling is superior to that of other methods, and the filtering error is minimum. In addition, as can be seen from fig. 3 and fig. 4, the errors of the three comparison algorithms all increase along with the increase of the iteration times, but the CTPF filtering performance of the invention is superior to that of the traditional resampling particle filtering algorithm, and the main reason is that the CTPF algorithm considers the particle depletion problem, and the ChopThin resampling is used, so that the particle depletion phenomenon is avoided by generating a group of particles with unequal weights, the filtering performance is stronger, the filtering effect is more stable, and the method is more suitable for tracking the moving target in the complex mine environment.
The foregoing is only a preferred embodiment of the invention, it being noted that: it will be apparent to those skilled in the art that various modifications and adaptations can be made without departing from the principles of the present invention, and such modifications and adaptations are intended to be comprehended within the scope of the invention.
Claims (5)
1. A target positioning and tracking method for a mine non-line-of-sight scene is characterized by comprising the following specific steps:
step one, determining a positioning tracking range and the number of base stations: firstly, determining a range area which is needed to be subjected to target positioning and tracking under a mine, and determining the number of base stations in the range area according to the range area;
Step two, determining a static target positioning model in a sight distance environment and a non-sight distance environment: setting the number of base stations as M, the coordinates as z i＝[xi yi]T, i=1, 2, & M, and numbering in sequence; the coordinate of the target to be measured is z= [ x y ] T; taking the base station with the number of 1 as a reference station, and respectively establishing a distance difference equation set from a target to other base stations i and the reference base station in a line-of-sight environment and a non-line-of-sight environment;
Judging whether non-line-of-sight errors exist in the communication positioning process of each base station or not: let the measurement error e i,1 obey the mean value 0 and the variance 0 Gaussian distribution, mean of non-line-of-sight error n i,1 of u ni,1, variance of/>Its standard deviation is σ ni,1; setting a base station vector A= [ A 1 A2 ... Ai ] with non-line-of-sight errors, wherein A i epsilon [0,1]; if a i =1, determining the ith base station as the base station with the non-line-of-sight error, otherwise, the base station does not have the non-line-of-sight error; setting a correction factor vector alpha= [ alpha 1 α2... αi ], wherein the non-line-of-sight error received by an ith base station is alpha iμni,1, giving an initial value to the correction factor vector alpha i in the first judgment, and obtaining the value of the correction factor vector alpha i in the subsequent judgment through updating and solving; when alpha iσni,1 is more than or equal to 0.5, at the moment, A i =1, and the ith base station is the base station with non-line-of-sight error; according to the step, whether the non-line-of-sight errors exist in the positioning communication process of each base station can be judged;
fourthly, static target positioning is carried out in the view distance scene: according to the judging result of the step three, if no non-line-of-sight error exists in the communication positioning process of each base station, solving a distance difference equation set from the target to other base stations i and the reference base station in the line-of-sight environment obtained in the step two by adopting a Chan algorithm, thereby obtaining the target position in the line-of-sight scene, completing the static target positioning process and entering the step six; if the non-line-of-sight errors exist in the communication positioning process of each base station, entering a step five;
fifthly, static target positioning is carried out in a non-line-of-sight scene: according to the judging result of the step three, if the non-line-of-sight errors exist in the communication positioning process of each base station, correcting a distance difference equation set from the target to other base stations i and the reference base station in the non-line-of-sight environment obtained in the step two through a non-line-of-sight error mean value u ni,1 and a base station vector A i of the non-line-of-sight errors, then solving the corrected distance difference equation set by adopting a Chan algorithm, thereby obtaining the target position in the non-line-of-sight scene, completing the static target positioning process and entering the step six;
step six, tracking the subsequent moving process of the target: the state estimation of the target moving process is carried out by adopting an improved particle filtering algorithm, specifically:
the improved particle filtering algorithm is based on state and observation equations, i.e
xk＝Fxk-1+Qk-1
zk＝h(xk)+Rk
Wherein F is a state transition matrix, and Q k、Rk is state transition noise and observation noise respectively; taking the corrected distance difference value obtained in the step five as the observed quantity of the improved particle filtering algorithm;
The improved particle filter algorithm comprises the following specific steps:
① Initializing: sampling from the a priori distribution p (x 0) to obtain a set of primary particles at time k=0 Setting particle weight/>
② Prediction stage: because of the nonlinearity of the equation, the posterior probability cannot be directly sampled, so an importance distribution q (x k|x0：k-1) convenient for sampling is introduced, namely
③ Updating: when data z is observed, the weight of each particle is updated and normalized:
Wherein, For likelihood distribution,/>Is a state transition distribution;
At this time, the posterior probability density is The new particle group is/>
④ Calculating the effective particle number and setting a threshold value: defining the effective particle number as N eff, setting a threshold value for reducing the operation amount of a particle filtering algorithmIf N eff＜Nt is not found, go to step ⑤, otherwise go to step ⑥;
Wherein round is a rounding operation;
⑤ Resampling: carrying out ChopThin resampling to obtain a group of particles with unequal weights;
⑥ Output state estimation: and obtaining the state estimation of each moment of the target according to the obtained data by the following formula:
thereby realizing the process of tracking the moving track when the target moves.
2. The method for positioning and tracking the target in the mine non-line-of-sight scene according to claim 1, wherein the distance difference equation set between the target and the other base station i and the reference base station in the line-of-sight environment in the second step is:
The distance difference equation set between the target and other base stations i and the reference base station in the non-line-of-sight environment is as follows:
where e i,1、ni,1 denotes the measurement error and the non-line-of-sight error, respectively, c is the electromagnetic wave propagation speed, τ i,1 is the arrival time difference between the target position to the other base station i and the reference base station.
3. The method for target positioning and tracking for mine non-line-of-sight scene as set forth in claim 1, wherein the updating and solving process of the correction factor vector α i in the third step is as follows:
Defining the residual function of the i-th base station distance difference r i,1 as:
the formula carries out derivation on alpha i to lead the derivative to be 0, and update solution of the correction factor alpha i is completed; wherein the method comprises the steps of And updating the correction factor alpha i after the current target position coordinate is determined.
4. The method for target positioning and tracking for mine non-line-of-sight scenes according to claim 1, wherein the fifth step is specifically:
Since the non-line-of-sight error does not follow zero mean distribution, the ranging result is corrected as follows:
and (3) carrying out correction on the distance difference value by the distance difference value equation set under the non-line-of-sight environment obtained in the step (II), wherein the corrected formula is as follows:
let the z be a＝[x y R]T to be the same as the first, Assuming that the linearity of x, y and R is irrelevant, then:
and (3) finishing to obtain:
wherein x i,1＝xi-x1, i=1, 2,;
The corrected distance difference value is carried into a Chan algorithm to calculate a target position, and an error vector is defined by considering non-line-of-sight errors and measurement errors:
Wherein,
The error vector has the following statistical features:
Wherein the method comprises the steps of ,B＝diag{[r2 r3 ... rM]},e＝[e2,1 e3,1 ... eM,1]T,n＝[n2,1 n3,1 ... nM,1]T;
The covariance matrix of the noise is:
the WLS algorithm is used for the first time to obtain an initial solution of the target:
Is the result obtained assuming that x, y, and R are independent of each other in z a＝[x y R]T, and in reality R is a quantity related to (x, y); in order to obtain a more accurate positioning result, a second WLS estimation is performed by using the initial solution obtained for the first time as a known constraint condition;
Order the The error vector becomes:
and (4) obtaining by using WLS estimation again:
Wherein the method comprises the steps of
The position of the target is finally obtained as follows:
thereby obtaining the position of the stationary object in the non-line-of-sight scene.
5. The target positioning and tracking method for non-line-of-sight scenes of a mine according to claim 1, wherein the ChopThin resampling in the sixth step is as follows:
defining the weight ratio of particles as eta, the threshold value as a and the number of particle offspring with weight as w as And meet/>Wherein N is the number of particles before resampling, and N is the target number of particles; the specific steps of resampling are:
1) Determining a threshold value a: a is used as a threshold value to determine whether particles are subjected to a Chop process or a Thin process, and the determination method of a is as follows:
Dividing the weight value w into two sets of w l and w u, uniformly distributing and sampling w l or w u to obtain a, and making Calculating the resampled particle number:
Wherein s l is the sum of the particle weights with the weight value greater than or equal to a, s u is the sum of the particle weights with the weight value greater than or equal to b, c m is the number of particles with the weight value greater than or equal to a, and c u is the number of particles with the weight value greater than or equal to b;
If h=n, return a, i.e Otherwise, the element is deleted from w l or w u and the weight sum is updated until h=n is satisfied;
2) The Thin process comprises the following steps: determining new weight and offspring number of small weight particles (w i < a), wherein the offspring number of the particles is determined by Carrying out system resampling and determining, wherein the weight of offspring particles is set as a, the number of particles processed by Thin is N L, and the total weight of the particles is aN L;
3) Setting a weight adjustment parameter K: the step 2) obtains that the total weight is possibly changed, and K is used for adjusting the total weight before the loop processing to ensure that the total weight is unchanged
Wherein l= { j: w j＜a},U＝{j：wj is greater than or equal to a };
4) In the Chop process, large-weight particles (w i. Gtoreq.a) are chopped, each large-weight particle is obtained Offspring, wherein m i is pair/>The fractional part of (a) is re-sampled twice, the particle weight range of the stage is (a, eta a), the total number of returned particles is N m, and N m+NL =N is satisfied.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210732436.4A CN115248415B (en) | 2022-06-27 | 2022-06-27 | Target positioning and tracking method for mine non-line-of-sight scene |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210732436.4A CN115248415B (en) | 2022-06-27 | 2022-06-27 | Target positioning and tracking method for mine non-line-of-sight scene |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115248415A CN115248415A (en) | 2022-10-28 |
| CN115248415B true CN115248415B (en) | 2024-05-14 |
Family
ID=83699838
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210732436.4A Active CN115248415B (en) | 2022-06-27 | 2022-06-27 | Target positioning and tracking method for mine non-line-of-sight scene |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115248415B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2012149624A1 (en) * | 2011-05-04 | 2012-11-08 | Jacques Georgy | Two-stage filtering based method for multiple target tracking |
| WO2018119912A1 (en) * | 2016-12-29 | 2018-07-05 | 深圳大学 | Target tracking method and device based on parallel fuzzy gaussian and particle filter |
| CN108802692A (en) * | 2018-05-25 | 2018-11-13 | 哈尔滨工程大学 | A kind of method for tracking target based on maximum cross-correlation entropy volume particle filter |
| CN108983215A (en) * | 2018-05-25 | 2018-12-11 | 哈尔滨工程大学 | A kind of method for tracking target based on maximum cross-correlation entropy adaptively without mark particle filter |
| CN111711432A (en) * | 2020-06-16 | 2020-09-25 | 桂林理工大学 | Target tracking algorithm based on UKF and PF hybrid filtering |
-
2022
- 2022-06-27 CN CN202210732436.4A patent/CN115248415B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2012149624A1 (en) * | 2011-05-04 | 2012-11-08 | Jacques Georgy | Two-stage filtering based method for multiple target tracking |
| WO2018119912A1 (en) * | 2016-12-29 | 2018-07-05 | 深圳大学 | Target tracking method and device based on parallel fuzzy gaussian and particle filter |
| CN108802692A (en) * | 2018-05-25 | 2018-11-13 | 哈尔滨工程大学 | A kind of method for tracking target based on maximum cross-correlation entropy volume particle filter |
| CN108983215A (en) * | 2018-05-25 | 2018-12-11 | 哈尔滨工程大学 | A kind of method for tracking target based on maximum cross-correlation entropy adaptively without mark particle filter |
| CN111711432A (en) * | 2020-06-16 | 2020-09-25 | 桂林理工大学 | Target tracking algorithm based on UKF and PF hybrid filtering |
Non-Patent Citations (1)
| Title |
|---|
| NLOS环境下的高斯-粒子滤波无线跟踪技术;毛永毅;燕琳凯;;西安邮电大学学报;20160710(04);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115248415A (en) | 2022-10-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106912105B (en) | Three-dimensional positioning method based on PSO _ BP neural network | |
| CN109188355B (en) | Method for optimizing and optimally distributing receiving antennas of multi-point positioning system | |
| KR20010095092A (en) | Location determination using weighted ridge regression | |
| CN110673089B (en) | Positioning method based on arrival time under unknown line-of-sight and non-line-of-sight distribution condition | |
| CN110636436A (en) | Three-dimensional UWB indoor positioning method based on improved CHAN algorithm | |
| CN115524661A (en) | Short wave time difference positioning method for joint optimization of height of ionized layer and target position | |
| CN115379558A (en) | Mobile station position estimation method based on particle swarm optimization crow algorithm | |
| CN109752690B (en) | Method, system and device for eliminating NLOS (non-line of sight) positioned by unmanned aerial vehicle and storage medium | |
| CN115248415B (en) | Target positioning and tracking method for mine non-line-of-sight scene | |
| Zhou et al. | Optimal location method of spontaneous data fusion based on TDOA/AOA | |
| CN116908778A (en) | Track section positioning method, track section positioning device, electronic equipment and medium | |
| Landolsi et al. | TOAI/AOA/RSS maximum likelihood data fusion for efficient localization in wireless networks | |
| Verma et al. | Doa estimation for autonomous systems in complex propagation environments | |
| CN112904275B (en) | Short wave sky wave propagation time difference positioning method based on Taylor series linear distance | |
| CN113852433B (en) | Wireless channel shadow fading model prediction method based on computer vision | |
| CN115866517A (en) | Indoor positioning system based on empirical mode decomposition and spatial autoregressive neural network | |
| Liao et al. | An indoor positioning algorithm against NLOS error based on UWB | |
| Liang et al. | Application of Taylor-Chan algorithm based on TDOA in sound source location | |
| CN116321410A (en) | Iterative weighted least square ultra-wideband positioning algorithm based on TOA | |
| CN115278870A (en) | Improved genetic ant colony hybrid positioning method and device based on TDOA selection | |
| Ma et al. | A TDOA localization method for complex environment localization | |
| CN110687502B (en) | Short wave direction finding data set labeling method based on least square positioning | |
| KR100789420B1 (en) | Method for accuracy enhancement for uwb indoor non-los location | |
| Wu et al. | A Regional Positioning Algorithm for Non-line-of-sight (NOLS) Propagation | |
| CN116203499A (en) | Intelligent factory UWB positioning shielding detection method |
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 |