CN115307644A - Three-dimensional positioning model based on UWB - Google Patents
Three-dimensional positioning model based on UWB Download PDFInfo
- Publication number
- CN115307644A CN115307644A CN202211022690.1A CN202211022690A CN115307644A CN 115307644 A CN115307644 A CN 115307644A CN 202211022690 A CN202211022690 A CN 202211022690A CN 115307644 A CN115307644 A CN 115307644A
- Authority
- CN
- China
- Prior art keywords
- data
- target point
- uwb
- point
- positioning
- 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.)
- Pending
Links
- 238000012360 testing method Methods 0.000 claims abstract description 32
- 238000000034 method Methods 0.000 claims abstract description 31
- 238000001914 filtration Methods 0.000 claims abstract description 20
- 238000004140 cleaning Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 33
- 230000002159 abnormal effect Effects 0.000 claims description 21
- 230000008569 process Effects 0.000 claims description 17
- 238000005259 measurement Methods 0.000 claims description 15
- 238000012937 correction Methods 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 7
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 238000011160 research Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 7
- 238000012544 monitoring process Methods 0.000 description 6
- 238000004891 communication Methods 0.000 description 5
- 238000005457 optimization Methods 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000000691 measurement method Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 231100001261 hazardous Toxicity 0.000 description 1
- 230000014759 maintenance of location Effects 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000576 supplementary effect Effects 0.000 description 1
Images
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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- General Engineering & Computer Science (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Probability & Statistics with Applications (AREA)
- Operations Research (AREA)
- Evolutionary Biology (AREA)
- Bioinformatics & Computational Biology (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Automation & Control Theory (AREA)
- Computing Systems (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The invention discloses a UWB-based three-dimensional positioning model, which comprises the following steps: setting a reference point in an application environment, and carrying out three-dimensional environment space; setting n UWB anchor points at the edge position, wherein n is a natural number more than or equal to 3; setting a test target point, and collecting the distance between an anchor point and the target point by using a UWB technology; classifying and cleaning data, and removing useless data; and establishing a positioning model by combining a Chan algorithm, a Taylor series positioning algorithm and a Kalman filtering method, and carrying out three-dimensional positioning on any object in the environment through the model. The positioning model is established by combining data acquisition, classification, positioning and noise reduction algorithms, the deviation of the model algorithm is corrected by adopting the test target point, the positioning model can be widely applied to various goods indoor positioning problems, the goods positions at different moments are monitored and positioned, the goods historical movement track can be reproduced according to the time sequence of the acquired data, and the positioning model has higher accuracy, effectiveness and applicability.
Description
Technical Field
The invention relates to the field of indoor positioning, in particular to a UWB-based three-dimensional positioning model.
Background
Ultra Wideband (UWB) is a short-range wireless communication technology that performs data transmission by transmitting nanosecond pulses without any carrier, and power consumption during signal transmission is only several tens of μ W. The accurate indoor positioning of UWB will play an excellent supplementary effect to satellite navigation, can have wide application in military and civilian field, for example: electric power, medical treatment, chemical industry, tunnel construction, hazardous area management and control etc. The positioning principle of the UWB technology is to place a plurality of UWB anchor points in a test environment, and the anchor points send signals to all directions. The target is a UWB tag, i.e. the object that needs to be located (only within the scope of the test environment). The target point receives the signal of the UWB anchor point, and then the distance data between the target point and each anchor point is calculated respectively.
In indoor positioning applications, UWB technology can achieve centimeter-level positioning accuracy, but has the following disadvantages:
1. UWB collected data are distance data of each anchor point and a target point, and cannot be directly used for positioning and monitoring of the target point;
2. because the indoor environment is complex and changeable, UWB communication signals are easy to be shielded;
3. when strong interference exists, data can fluctuate abnormally, and indoor positioning cannot be completed basically.
Disclosure of Invention
In view of the above problems, the present invention aims to provide a UWB-based three-dimensional positioning model. The model overcomes the defects of the prior art, acquires data signals by using a UWB wireless communication technology, classifies and filters abnormal data and interference data, establishes a positioning model by combining a Chan algorithm, a Taylor series positioning algorithm and a Kalman filtering method, corrects the error by adopting a test target point, solves UWB distance data into a space three-dimensional position of a target object, and can draw a motion track of the target point according to an acquisition time sequence. The method has higher accuracy, effectiveness and applicability, can well solve the problems of indoor object positioning and moving track monitoring, and can be used for different actual scenes.
The technical scheme provided by the invention for solving the technical problems is as follows: a UWB-based three-dimensional positioning model comprising the steps of:
s1, setting a reference point in an application environment, and three-dimensionally transforming a field space based on the reference point.
S2, setting n UWB anchor points at the edge positions, and transmitting signals to all directions around the UWB anchor points.
Preferably, n is a natural number of 3 or more.
And S3, setting a test target point, wherein the target point can receive the signal transmitted by the anchor point and reflect the signal to the anchor point.
Preferably, time of flight (TOF) ranging is adopted, and the distance between the anchor point and the target point can be obtained according to the time difference between the signals transmitted and received by the anchor point and the signal propagation speed, wherein the distance can be obtained according to the following formula:
d＝c×Δt/2 (1)
in the formula: d is the distance between the anchor point and the target point, and is mm; c is the speed of light, and is taken to be 3X 10 5 km/s; Δ t is the time difference, s, between the anchor point transmit and receive signals.
And S4, classifying and cleaning the data, eliminating useless data, improving the data precision and reducing the operation time and workload. The signal can be sent and received once every 0.2-0.3 seconds between the anchor point and the target point, at the same position point, the UWB can acquire a plurality of groups of data, and the longer the stay time of the target point at the same position is, the more the groups are. Therefore, during the acquisition process, the problems of target point residence time, obstacles, signal interference and the like can generate a lot of useless (abnormal, missing and identical) data. The distance between the target point and the anchor point is recorded as
(i =1,2,3,. Cndot., n; k =1,2,3,. Cndot., m, n is the number of anchor points, m is the number of sample groups).
Preferably, the definition of the abnormal data is as follows: for a data file at a certain target point position, a plurality of groups of sample data exist, and according to the 3 sigma criterion, if the deviation of certain sample data and the average value exceeds two times of standard deviation, the group of data is called abnormal data. When the deviation exceeds three times the standard deviation, it is called a highly abnormal value.
Preferably, the missing data is defined as: for a data file at a certain target position, a plurality of groups of sample data exist, and if the anchor point ranging value measured by a certain group of sample data is null/0, the group of data is regarded as missing data.
Preferably, the definition of the same data is as follows: for a data file at a certain target point position, a plurality of groups of sample data exist, one group of sample data is used as a research object, and if the distance between the anchor point of the kth group of data and the target point is the same as that of the kth group of data, namely the distance between the anchor point of the kth group of data and the target point of the kth group of data corresponds to that of the kth group of data, the data file at the target point position has a plurality of groups of sample data
(k, k '=1,2, 3.,. N, and k ≠ k'), it is regarded that the kth group of sample data and the kth group of sample data are the same data.
The mathematical model of the garbage can be expressed as:
in the formula:
is an average value;
is the standard deviation.
S5, establishing a positioning model, and correcting data by using a test target spot to improve positioning accuracy, wherein the positioning model is determined through the following substeps:
and S51, calculating the initial estimation position of the target point by utilizing a Chan algorithm. Setting the positioning coordinate T (x, y, z) of the test target point and the anchor point coordinate position A i (x i ,y i ,z i ) Distance measurement of target and anchor points d i . UWB measurement techniques typically result in range errors, i.e., d, due to interference, temperature drift, capacitive coupling, and other problems i ≠(x-x i ) 2 +(y-y i ) 2 +(z-z i ) 2 . Assuming that the spatial distance between the test target and the anchor point is
Then there are:
for a certain set of target points, the spatial distance from the anchor point can be expressed as:
preferably, the range and spatial distance errors of the target point are required to be as small as possible. Suppose that the distance measurement error for each anchor point is Δ d i Then there is
Let the error be ε = (ε) i ) n×1 Then, there are:
preferably, it is usually
Therefore it has the advantages of
Then, there are:
ε≈2D(Δd) (7)
in the formula:
Δd＝[Δd 1 Δd 2 … Δd n ]。
preferably, let σ be the error threshold and covariance matrix of the measured error vector Δ d be
Then the covariance matrix of the error vector epsilon is:
φ＝E(εε T )＝4DE(Δd(Δd) T )D＝4DQD (8)
as an optimization, the initial estimated position of the target point is set as
In order to minimize the error, a weighted least square method is used for calculation, and the calculation formula is as follows:
s52, calculating the positioning residual error of the initial estimation position by using a non-line-of-sight residual error identification method, and then comparing the positioning residual error of the initial estimation position with a given threshold value to obtain the observation position of the target point.
Preferably, if the positioning residual value of the initial estimated position is less than or equal to a given threshold value, the position is accurate enough and is called an observed position; otherwise, iterative solution of the observation position by using a Taylor series algorithm is required.
As an option, setting the initial estimated position of the target point
And the precise coordinate (x) 0 ,y 0 ,z 0 ) An error vector corresponding to the error of each component is δ = (Δ x, Δ y, Δ z) T . Spatial distance is expressed in
The first order expansion is performed using the Taylor formula, which has:
in the formula:
the spatial distance between the initial estimated position of the target point and the anchor point is set; d is a radical of i0 The space distance between the precise coordinate of the target point and the anchor point is used.
Preferably, the error can be expressed as = (epsilon) i ) n×1 . Order:
the iterative formula of the error vector obtained by the weighted least square method is as follows:
in the formula: q is the covariance matrix of the measurement error.
Preferably, the error vector δ is obtained by an iterative equation
Of the value of (c), and comparing this value with a threshold value
A comparison is made. If it is
If the value is less than the threshold value, the position is accurate enough, and the (x, y, z) is called an observation position; otherwise, the iterative formula of coordinates is needed
Performing iterative calculation until
Less than the threshold.
And S53, removing ranging noise and interference noise by using a Kalman filtering algorithm to improve positioning accuracy, and finally obtaining the predicted position of the target point.
Preferably, the kalman filtering is divided into a prediction process and an update process, and the prediction process is as follows:
in the formula: x k,k-1 An estimate representing a state matrix at time k; x k-1 An estimated value representing a state matrix at the time k-1; f k-1 A state transition matrix from the moment k-1 to the moment k; p is k-1 Representing the state covariance matrix at time k-1; p k,k-1 A state covariance matrix representing time k; process noise w k ～N(0,Q k )。
Preferably, the updating iteration of the Kalman filtering algorithm is utilized to obtain the predicted position
The updating process is as follows:
in the formula: observation vector Z k ＝H k X k,k-1 +v k ，
Observation noise v k ～N(0,R k )；K k Is the Kalman gain; r k To observe the noise matrix.
As an optimization, aiming at abnormal conditions, the adaptive robust Kalman filtering is provided. The idea is to carry out covariance R on the basis of Kalman filtering algorithm k ，Q k The correction is made. The correction process is as follows:
observation noise v k ＝Z k -H k X k,k-1 The covariance matrix is
When the non-line-of-sight error is not considered and the measurement environment condition is better, v is k ～N(0,R k ). However, when there is interference between the target point and the anchor point, the range finding may have an abnormal value, which may be considered as
Wherein
Is an accurate distance. Thus, the conditions for checking whether the measurement value is abnormal may be set as:
in the formula: c is a threshold value;
for R k The correction formula of (c) is as follows:
preferably, the covariance matrix of the system noise is estimated in real time by using improved Sage-Husa filtering:
in the formula: alpha is alpha k ＝(1-b)/(1-b k+1 ) And the forgetting factor 0 < b < 1.
S6, positioning any object in the current application environment according to the positioning model, and monitoring and reproducing the motion trail according to the time sequence of the positioning target data acquisition.
Compared with the prior art, the invention has the following beneficial effects:
the UWB-based three-dimensional positioning model solves the problem of signal interference of a UWB wireless communication technology, filters useless data, reduces operation time and workload, and improves data accuracy. The position of the target point is positioned by combining a Chan algorithm, a Taylor series positioning algorithm and a Kalman filtering method, so that the positioning precision is improved. The motion trail of the target point can be reproduced according to the time sequence of the acquired data. The method reduces the operation time, improves the accuracy of the positioning model, and can well solve the problems of indoor object positioning and moving track monitoring.
Drawings
To illustrate the technical solutions of the embodiments of the present invention more clearly, the drawings of the embodiments will be briefly introduced, and it is obvious that the drawings in the following description only relate to some embodiments of the present invention, and are not to limit the present invention.
FIG. 1 is a diagram of a test environment anchor placement, according to one embodiment.
FIG. 2 is a schematic flow chart diagram of a positioning model according to an embodiment.
The reference numbers in the figures:
1-testing target point, 2-UWB anchor point.
Detailed Description
The invention is further illustrated by the following examples in conjunction with the drawings. It is noted that, unless otherwise indicated, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.
In the present invention, the terms "first", "second", and the like are used for distinguishing similar objects, but not for describing a particular order or sequence order, unless otherwise specified. It is to be understood that the terms so used; the terms "upper", "lower", "left", "right", and the like are used generally with respect to the orientation shown in the drawings, or with respect to the component itself in a vertical, or gravitational orientation; likewise, "inner", "outer", and the like refer to inner and outer relative to the contours of the components themselves for ease of understanding and description. The above directional terms are not intended to limit the present invention.
The invention provides a UWB-based three-dimensional positioning model, which comprises the following steps:
s10, setting a reference point in the application environment, and three-dimensionally transforming the field space based on the reference point.
In one particular embodiment, the reference points and the spatial three-dimensional coordinate system are as shown in FIG. 1.
And S20, setting n UWB anchor points at the edge positions, and transmitting signals to all directions around by the UWB anchor points.
In one particular embodiment, as shown in FIG. 1. 4 UWB anchor points are set in the application environment.
And S30, setting a test target point, wherein the target point can receive the signal transmitted by the anchor point and reflect the signal to the anchor point.
Preferably, UWB employs time of flight (TOF) ranging, and the distance between an anchor point and a target point can be obtained according to the time difference between signals transmitted and received by the anchor point and the signal propagation speed, wherein the distance can be obtained by the following formula:
d＝c×Δt/2 (1)
in the formula: d is the distance between the anchor point and the target point, mm; c is the speed of light, and is taken to be 3X 10 5 km/s; Δ t is the time difference, s, between the anchor point transmitted and received signals.
In a specific embodiment, a set of sample data obtained by collection is represented as:
T:104825146:RR:0:0:1320:1320:21:3349
T:104825146:RR:0:1:3950:3950:21:3349
T:104825146:RR:0:2:4540:4540:21:3349
T:104825146:RR:0:3:5760:5760:21:3349
( Tag identification: time stamping: abbreviation for Range Report: tag ID: and anchor ID: range value (mm) of the anchor point: and (3) checking the value: data sequence number: data numbering )
S40, data classification and cleaning are carried out, useless data are eliminated, data accuracy is improved, and operation time and workload are reduced. The signal will be sent and received once every 0.2-0.3 second between anchor point and target point, at the same position point, UWB will gather the multiunit data, the stop of target point at the same positionThe longer the dwell time, the more groups. Therefore, during the acquisition process, the problems of target retention time, obstacles, signal interference and the like can generate a lot of useless (abnormal, missing and identical) data. The distance between the target point and the anchor point is recorded as
(i =1,2,3,. Cndot., n; k =1,2,3,. Cndot., m, n is the number of anchor points, m is the number of sample groups).
Preferably, the definition of the anomaly data is as follows: for a data file at a certain target point position, a plurality of groups of sample data exist, and according to the 3 sigma criterion, if the deviation of certain sample data and the average value exceeds two times of standard deviation, the group of data is called abnormal data. When the deviation exceeds three times the standard deviation, it is called a highly abnormal value.
Preferably, the missing data is defined as: for a data file at a certain target point position, a plurality of groups of sample data exist, and if the anchor point ranging value measured by a certain group of sample data is null/0, the group of data is regarded as missing data.
Preferably, the definition of the same data is as follows: for a data file at a certain target point position, a plurality of groups of sample data exist, one group of sample data is used as a research object, and if the distance between an anchor point of the kth group of data and the target point is the same as that of the kth group of data, namely the data is stored in a storage unit, and the data is stored in a storage unit
(k, k '=1,2, 3.,. N, and k ≠ k'), it is regarded that the kth group of sample data and the kth group of sample data are the same data.
The mathematical model of the garbage can be expressed as:
in the formula:
is an average value;
is the standard deviation.
S5, establishing a positioning model, and correcting data by using a test target spot to improve positioning accuracy, wherein the positioning model is determined through the following substeps:
and S51, calculating the initial estimated position of the target point by utilizing a Chan algorithm. Setting the positioning coordinate T (x, y, z) of the test target point and the anchor point coordinate position A i (x i ,y i ,z i ) Distance measurement of target point to be measured and anchor point d i . UWB measurement techniques typically result in range errors, i.e., d, due to interference, temperature drift, capacitive coupling, and other problems i ≠(x-x i ) 2 +(y-y i ) 2 +(z-z i ) 2 。
In one particular embodiment, as shown in FIG. 1. The coordinates of the 4 UWB anchor points are: a0 (0, 1300), A1 (5000, 0, 1700), A2 (0, 5000, 1700), A3 (5000, 5000, 1300).
Preferably, the spatial distance between the target point and the anchor point is assumed to be
Can be expressed as:
in a specific embodiment, the spatial distance between the target point and the anchor point can be expressed as:
let X α ＝[x y z R] T Then, the following formula can be obtained:
G α X α ＝H α (5)
in the formula:
preferably, the distance measurement and the spatial distance from the target point to each anchor point are required to be as small as possible, namely min | (x-x) i ) 2 +(y-y i ) 2 +(z-z i ) 2 -d i | | the present inventors have studied. Suppose that the distance measurement error for each anchor point is Δ d i Then there is
Let the error be ε = (ε) i ) n×1 Then, there are:
preferably, it is usually
Therefore it has the advantages of
Then, there are:
ε≈2D(Δd) (7)
in the formula:
Δd＝[Δd 1 Δd 2 Δd 3 Δd 4 ] T 。
preferably, let σ be the error threshold, and the covariance matrix of the measured error vector Δ d be
Then the covariance matrix of the error vector epsilon is:
φ＝E(εε T )＝4DE(Δd(Δd) T )D＝4DQD (8)
as an option, the initial estimated position of the target point is set as
In order to minimize the error, a weighted least squares calculation is applied,the calculation formula is:
s52, calculating the positioning residual error of the initial estimation position by using a non-line-of-sight residual error identification method, and then comparing the positioning residual error of the initial estimation position with a given threshold value to obtain the observation position of the target point.
Preferably, if the positioning residual value of the initial estimated position is less than or equal to a given threshold value, the position is accurate enough and is called an observed position; otherwise, the Taylor series positioning algorithm is needed to be used for iterative solution of the observation position.
As an option, setting the initial estimated position of the target point
And the precise coordinate (x) 0 ,y 0 ,z 0 ) An error vector corresponding to the error of each component is δ = (Δ x, Δ y, Δ z) T . Expressing the spatial distance in
The first order expansion is performed using the Taylor formula, which has:
in the formula:
the spatial distance between the initial estimated position of the test target point and the anchor point is obtained; d is a radical of i0 The space distance between the precise coordinate of the test target point and the anchor point;
preferably, the error can be expressed as = (epsilon) i ) n×1 . Order:
the iterative formula for obtaining the error vector by the weighted least squares method is as follows:
in the formula: q is the covariance matrix of the measurement error.
Preferably, the error vector δ is obtained by an iterative equation
And comparing this value with a threshold value
A comparison is made. If it is
If the value is less than the threshold value, the position is accurate enough, and the (x, y, z) is called an observation position; otherwise, iterative formula of coordinate is needed
Performing iterative calculation until
Less than the threshold.
And S53, removing ranging noise and interference noise by using a Kalman filtering algorithm to improve positioning accuracy, and finally obtaining the predicted position of the target point.
Preferably, the kalman filtering is divided into a prediction process and an update process, and the prediction process is as follows:
in the formula:
estimation of state matrix representing at time kEvaluating;
the estimated value of the state matrix at the k-1 moment is the coordinate (x, y, z) calculated by the Taylor method; f k-1 Is the state transition matrix from time k-1 to time k, F k-1 ＝I 3 ；P k-1 Representing the state covariance matrix at time k-1; p is k,k-1 Representing the state covariance matrix at time k, P 0 ＝I 3 (ii) a Process noise w k ～N(0,Q k )，
As an optimization, the prediction position is obtained by utilizing the update iteration of the Kalman filtering algorithm
The updating process is as follows:
in the formula: observation vector
Observation noise v k ～N(0,R k ) (ii) a Kalman gain of K k The magnitude of which represents a more confident predictor
Or an observed value
Observing a noise matrix
As an optimization, aiming at abnormal conditions, the adaptive robust Kalman filtering is provided. The idea is to carry out covariance R on the basis of Kalman filtering algorithm k ，Q k The correction is made. The correction process is as follows:
observing noise
The covariance matrix is
When the non-line-of-sight error is not considered and the measurement environment condition is better, v is k ～N(0,R k ). However, when there is interference between the target point and the anchor point, the range finding may have an abnormal value, which may be considered as
Wherein
Is an accurate distance. Thus, the conditions for checking whether the measurement value is abnormal may be set as:
in the formula: c is a threshold value;
for R k The correction formula of (2) is as follows:
preferably, the covariance matrix of the system noise is estimated in real time by using improved Sage-Husa filtering:
in the formula: alpha is alpha k ＝(1-b)/(1-b k+1 ) And forgetting factor b =0.5.
In a specific embodiment, the precise coordinates of the test target point are (450, 450, 200), the obtained predicted position is (453, 461, 219), and the coordinate error is within 20mm, so that the accuracy is high.
S60, positioning any object in the current application environment according to the positioning model, and monitoring and reproducing the motion trail according to the time sequence of data acquisition of the positioning target.
In a specific embodiment, the location of the target point is predicted by using the positioning model, and the predicted coordinates of the target point are obtained as shown in table 1.
TABLE 1 target prediction coordinate and dimensional accuracy of an embodiment
Compared with the prior art, the invention has the following beneficial effects:
the UWB-based three-dimensional positioning model can well solve the signal interference problem of the UWB wireless communication technology, and the positioning model is corrected by using the test target point, so that the positioning precision is improved. The scene space is three-dimensional, and the position monitoring and the reproduction of the moving track of any object can be performed according to the coordinate position and the acquisition time of the target object. During specific implementation, model rectification can be performed on an application scene through the anchor point and the test target point, so that the method can be well expanded and applied to different indoor environments, and has high accuracy.
Although the present invention has been described with reference to a preferred embodiment, it should be understood that various changes, substitutions and alterations can be made herein without departing from the spirit and scope of the invention as defined by the appended claims.
Claims (8)
1. A UWB-based three-dimensional positioning model is characterized in that the establishment of the three-dimensional positioning model comprises the following steps:
s1, setting a reference point in an application environment, and three-dimensionally transforming a field space based on the reference point;
s2, arranging 3 or more UWB anchor points at the edge positions, and transmitting signals to all directions around by the UWB anchor points;
s3, setting a test target point, and collecting the distance between an anchor point and the test target point;
s4, classifying and cleaning data, and removing useless data;
and S5, establishing a positioning model by combining a Chan algorithm, a Taylor series algorithm and a Kalman filtering algorithm, and correcting data by testing a target point.
2. The three-dimensional UWB-based positioning model of claim 1, wherein in step S3, the distance between the anchor point and the target point under test can be obtained according to the following formula:
d＝c×Δt/2 (1)
in the formula: d is the distance between the anchor point and the target point, and is mm; c is the speed of light, and is taken to be 3X 10 5 km/s; Δ t is the time difference, s, between the anchor point transmit and receive signals.
3. The three-dimensional UWB-based positioning model according to claim 1, wherein in step S4, the useless data comprises abnormal data, missing data and identical data, and the data is defined as follows:
the definition of the abnormal data is as follows: for a data file at a certain target point position, a plurality of groups of sample data exist, and if the deviation of certain sample data and the average value exceeds two times of standard deviation, the group of data is called abnormal data;
the missing data is defined as: for a data file at a certain target point position, a plurality of groups of sample data exist, and if the anchor point ranging value measured by a certain group of sample data is 0/empty, the group of data is regarded as missing data;
the definition of the same data is as follows: for a data file at a certain target point position, a plurality of groups of sample data exist, the whole group of sample data is used as a research object, and if the distances between anchor points of certain two groups of data and the target point are correspondingly the same, the data are regarded as the same data;
the mathematical model of the garbage data may be expressed as:
in the formula:
the distance between the target point and the anchor point is (i =1,2, 3.. Cndot; k =1,2, 3.. Cndot; m, n is the number of anchor points, and m is the number of sample groups);
is an average value;
is the standard deviation.
4. The three-dimensional UWB-based positioning model according to claim 1, wherein in step S5, the establishment of the positioning model comprises the following sub-steps:
s51, calculating an initial estimation position of a test target by utilizing a Chan algorithm;
s52, calculating a positioning residual error of the initial estimation position by using a non-line-of-sight residual error identification method, wherein if the positioning residual error value of the initial estimation position is less than or equal to a given threshold value, the position is accurate enough and is called as an observation position; otherwise, iterative solution of the observation position by using a Taylor series algorithm is required;
and S53, removing ranging noise and interference noise by using a Kalman filtering algorithm, and finally obtaining a predicted position.
5. The UWB-based three-dimensional positioning model of claim 4, wherein in step S51, the initial estimated position of the target point is obtained according to the following formula:
in the formula:
an initial estimated position for the test target; sigma is an error threshold value;
the spatial distance between the test target point and the anchor point is set; (x, y, z) are the positioning coordinates of the test target point; (x) i ,y i ,z i ) Is the anchor point coordinate position;
R＝x 2 +y 2 +z 2 ；d i ranging the test target point and the anchor point; Δ d i For the measurement error of the distance of each anchor point,
epsilon is an error vector; phi is a covariance matrix of the error vector epsilon;
Δd＝[Δd 1 Δd 2 …Δd n ](ii) a Q is the covariance matrix of the measurement error ad,
and sigma is an error threshold value.
6. The UWB-based three-dimensional positioning model of claim 4, wherein in step S52, the solution of the observation position is as follows:
step one, expressing the space distance in
The first order expansion is performed using the Taylor formula, which has:
step two, obtaining an iterative formula of an error vector through a weighted least square method as follows:
in the above formula:
an initial estimated position for the test target; (x) 0 ,y 0 ,z 0 ) Precise coordinates for the test target;
estimating the spatial distance between the initial position of the test target point and the anchor point; d i0 The space distance between the precise coordinate of the test target point and the anchor point; q is a covariance matrix of the measurement error;
step three, solving the error vector delta according to the iterative formula
And comparing this value with a threshold value
Make a comparison if
If the value is less than the threshold value, the position is accurate enough, and the (x, y, z) is called an observation position; otherwise, iterative formula of coordinate is needed
Performing iterative calculation until
Less than the threshold.
7. The UWB-based three-dimensional positioning model of claim 4, wherein in step S53, the solution of the predicted position is as follows:
step one, using a Kalman filtering algorithm to predict, wherein the prediction process can be expressed by the following formula:
step two, updating and iterating by using a Kalman filtering algorithm to obtain a predicted position
The solution equation is as follows:
in the above formula: x k,k-1 An estimate representing a state matrix at time k; x k-1 An estimated value representing a state matrix at the time k-1; f k-1 A state transition matrix from the time k-1 to the time k; p k-1 Representing the state covariance matrix at time k-1; p k,k-1 A state covariance matrix representing time k; process noise w k ～N(0,Q k )；Z k To observe the vector, Z k ＝H k X k,k-1 +v k ；v k To observe noise, v k ～N(0,R k )；K k Is the Kalman gain; r k To observe the noise matrix.
8. The UWB-based three-dimensional positioning model according to claim 7, wherein for abnormal situations, an adaptive robust Kalman filter is proposed for R k ，Q k The correction is carried out, and the correction process is as follows:
step one, judging whether the measured value is abnormal or not, and when the interference exists between the target point and the anchor point, the distance measurement can generate an abnormal value which can be considered as
Wherein
If the distance is accurate, the conditions for judging whether the measured value is abnormal are as follows:
step two, to R k And correcting according to the following formula:
and step three, estimating the covariance matrix of the system noise in real time by using improved Sage-Husa filtering:
in the above formula: c is a threshold value;
v k to watchMeasuring noise, v k ＝Z k -H k X k,k-1 ；D k In the form of a covariance matrix,
α k ＝(1-b)/(1-b k+1 ) And the forgetting factor 0 < b < 1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211022690.1A CN115307644A (en) | 2022-08-25 | 2022-08-25 | Three-dimensional positioning model based on UWB |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211022690.1A CN115307644A (en) | 2022-08-25 | 2022-08-25 | Three-dimensional positioning model based on UWB |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115307644A true CN115307644A (en) | 2022-11-08 |
Family
ID=83865095
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211022690.1A Pending CN115307644A (en) | 2022-08-25 | 2022-08-25 | Three-dimensional positioning model based on UWB |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115307644A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115680626A (en) * | 2022-11-15 | 2023-02-03 | 西南石油大学 | Dynamic liquid level monitoring equipment and method for horizontal separator |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108182306A (en) * | 2017-12-18 | 2018-06-19 | 中国北方车辆研究所 | Power train for vehicle abrasive grain characteristic parameter degradation failure threshold value determination method |
| CN109186609A (en) * | 2018-10-09 | 2019-01-11 | 南京航空航天大学 | UWB localization method based on KF algorithm, Chan algorithm and Taylor algorithm |
| CN110189051A (en) * | 2019-06-06 | 2019-08-30 | 北京百奥知信息科技有限公司 | A kind of appraisal procedure for the Journals influence power considering reference exceptional value |
| CN111862538A (en) * | 2020-08-03 | 2020-10-30 | 中铁二院工程集团有限责任公司 | Large wind early warning method and system for long-span arch bridge construction period |
| CN113516192A (en) * | 2021-07-19 | 2021-10-19 | 国网北京市电力公司 | Method, system, device and storage medium for identifying user electricity consumption transaction |
| CN114594421A (en) * | 2022-02-15 | 2022-06-07 | 湖北大学 | Moving target point position calculation method based on least square method and Kalman filter |
-
2022
- 2022-08-25 CN CN202211022690.1A patent/CN115307644A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108182306A (en) * | 2017-12-18 | 2018-06-19 | 中国北方车辆研究所 | Power train for vehicle abrasive grain characteristic parameter degradation failure threshold value determination method |
| CN109186609A (en) * | 2018-10-09 | 2019-01-11 | 南京航空航天大学 | UWB localization method based on KF algorithm, Chan algorithm and Taylor algorithm |
| CN110189051A (en) * | 2019-06-06 | 2019-08-30 | 北京百奥知信息科技有限公司 | A kind of appraisal procedure for the Journals influence power considering reference exceptional value |
| CN111862538A (en) * | 2020-08-03 | 2020-10-30 | 中铁二院工程集团有限责任公司 | Large wind early warning method and system for long-span arch bridge construction period |
| CN113516192A (en) * | 2021-07-19 | 2021-10-19 | 国网北京市电力公司 | Method, system, device and storage medium for identifying user electricity consumption transaction |
| CN114594421A (en) * | 2022-02-15 | 2022-06-07 | 湖北大学 | Moving target point position calculation method based on least square method and Kalman filter |
Non-Patent Citations (3)
| Title |
|---|
| 刘韬;徐爱功;隋心;: "基于自适应抗差卡尔曼滤波的UWB室内定位", 传感技术学报, no. 04, 27 April 2018 (2018-04-27), pages 567 - 572 * |
| 王鑫: "基于UWB的室内目标三维跟踪定位技术研究", 中国优秀硕士学位论文全文数据库 (信息科技辑), 15 November 2021 (2021-11-15), pages 136 - 13 * |
| 邓勃: "关于异常值的检验与处理", 大学化学, no. 04, 30 August 1995 (1995-08-30), pages 5 - 9 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115680626A (en) * | 2022-11-15 | 2023-02-03 | 西南石油大学 | Dynamic liquid level monitoring equipment and method for horizontal separator |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109884586A (en) | Unmanned plane localization method, device, system and storage medium based on ultra-wide band | |
| CN109597864B (en) | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering | |
| CN112533149B (en) | Moving target positioning algorithm based on UWB mobile node | |
| CA2974031A1 (en) | Method of tracking one or more mobile objects in a site and a system employing same | |
| CN104849702B (en) | Radar system error combined estimation method is filtered using the GM EPHD of ADS B datas | |
| CN112533163A (en) | Indoor positioning method based on NB-IoT (NB-IoT) improved fusion ultra-wideband and Bluetooth | |
| CN104101876B (en) | Random finite set based multi-target tracking method in outer radiation source radar | |
| CN106231552B (en) | A kind of noncooperative target localization method based on hybrid mode | |
| CN106932752A (en) | A kind of real-time indoor orientation method based on RF phse monitoring | |
| CN115307644A (en) | Three-dimensional positioning model based on UWB | |
| CN107561489B (en) | MLS passive direction finding positioning method based on anomaly detection | |
| CN114916059B (en) | WiFi fingerprint sparse map extension method based on interval random logarithmic shadow model | |
| Kong et al. | Hybrid indoor positioning method of BLE and monocular VINS based smartphone | |
| CN109917373A (en) | Tracking before the Dynamic Programming of the moving platform radar of motion compensation search detects | |
| CN109839633A (en) | Tracking before the multi frame detection of airborne early warning radar based on minimum vertex-covering airspace | |
| CN113008235A (en) | Multi-source navigation information fusion method based on matrix K-L divergence | |
| CN115421153B (en) | Laser radar and UWB combined positioning method and system based on extended Kalman filtering | |
| Wang et al. | Gaussian filtered rssi-based indoor localization in wlan using bootstrap filter | |
| CN116908872A (en) | Multi-robot laser radar map construction method combining WIFI similarity | |
| CN113891251B (en) | Inertial navigation-WiFi signal fusion positioning algorithm based on AGV | |
| CN112731321B (en) | Mobile robot obstacle avoidance and map drawing method based on MIMO cognitive radar | |
| CN115144851A (en) | Multi-station positioning tracking method based on pitch angle | |
| CN107390191A (en) | A kind of ECEF coordinate systems lower probability assumes density filtering radar space error method for registering | |
| CN114666732A (en) | Moving target positioning resolving and error evaluation method under noisy network | |
| CN114237208A (en) | Automatic path guiding method and system in unattended intelligent warehouse |
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 |