CN115307644A - A 3D Positioning Model Based on UWB - Google Patents

Abstract

The invention discloses a three-dimensional positioning model based on UWB. The steps are as follows: a reference point is set in an application environment to make the environment space three-dimensional; n UWB anchor points are set at edge positions, where n is a natural number greater than or equal to 3; Set the test target point, use UWB technology to collect the distance between the anchor point and the target point; classify and clean the data to eliminate useless data; combine the Chan algorithm, Taylor series positioning algorithm and Kalman filtering method to establish a positioning model, through this model, namely Three-dimensional positioning of any object in the environment can be performed. The invention combines data collection, classification, positioning and noise reduction algorithms to establish a positioning model, uses test targets to correct the deviation of the model algorithm, can be widely used in various indoor positioning problems of goods, monitors and locates the positions of goods at different times, and collects data according to the collected data. The chronological order of the time sequence can reproduce the historical movement trajectory of the object, and the positioning model has high accuracy, validity and applicability.

Description

A 3D Positioning Model Based on UWB

technical field

The invention relates to the field of indoor positioning, in particular to a UWB-based three-dimensional positioning model.

Background technique

Ultra-wideband (UWB) technology is a short-distance wireless communication technology that completes data transmission by sending nanosecond pulses without any carrier, and the power consumption during signal transmission is only tens of μW. The precise indoor positioning of UWB will play an excellent supplementary role to satellite navigation, and can be widely used in military and civilian fields, such as: electric power, medical treatment, chemical industry, tunnel construction, dangerous area control, etc. The positioning principle of UWB technology is to place several UWB anchor points in the test environment, and the anchor points send signals to all directions. The target is the UWB tag, that is, the target 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 calculates the distance data between the target point and each anchor point.

In the application of indoor positioning, UWB technology can achieve centimeter-level positioning accuracy, but there are still the following disadvantages:

1. The data collected by UWB is the distance data between each anchor point and the target point, which cannot be directly used for the positioning and monitoring of the target point;

2. Due to the complex and changeable indoor environment, UWB communication signals are easily blocked;

3. When there is strong interference, the data will fluctuate abnormally, and it is basically impossible to complete indoor positioning.

Contents of the 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 existing technology, uses UWB wireless communication technology to collect data signals, classifies and filters abnormal data and interference data, and combines Chan algorithm, Taylor series positioning algorithm and Kalman filtering method to establish a positioning model, adopts test The target point is corrected, and the UWB distance data is calculated into the spatial three-dimensional position of the target object, and the movement trajectory of the target point can be depicted according to the acquisition time sequence. This method has high accuracy, effectiveness and applicability, and can well solve the problems of indoor object positioning and moving track monitoring, and can also be used in different actual scenarios.

The technical solution provided by the present invention to solve the above-mentioned technical problems is: a three-dimensional positioning model based on UWB, comprising the following steps:

S1. Set a reference point in the application environment, based on which the site space is three-dimensionalized.

S2. Set n UWB anchor points at the edge, and the UWB anchor points transmit signals to all directions around.

Preferably, n is a natural number greater than or equal to 3.

S3. Setting a test target point, the target point can receive the signal transmitted from the anchor point and reflect it to the anchor point.

As an optimization, 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 anchor point transmitting and receiving signals and the signal propagation speed, and 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, which is 3×10 ⁵ km/s; Δt is the time difference between sending and receiving signals of the anchor point, s.

(i＝1，2，3，...，n；k＝1，2，3，...，m，n为锚点个数，m为样本组数)。S4. Data classification and cleaning, eliminating useless data, improving data accuracy, and reducing computing time and workload. The anchor point and the target point will send and receive signals every 0.2-0.3 seconds. At the same point, UWB will collect multiple sets of data. The longer the target point stays at the same position, the more the number of sets will be. many. Therefore, during the acquisition process, many useless (abnormal, missing, identical) data will be generated due to problems such as the residence time of the target, obstacles, and signal interference. Record the distance between the target point and the anchor point as

(i=1, 2, 3, . . . , n; k=1, 2, 3, . . . , m, n is the number of anchor points, and m is the number of sample groups).

As an optimization, the definition of abnormal data is: for the data file of a certain target point, there are several sets of sample data. According to the 3σ rule, if the deviation between a certain sample data and the average value exceeds two standard deviations, it is called This set of data is abnormal data. When the deviation exceeds three standard deviations, it is called a highly abnormal outlier.

As an optimization, the definition of missing data is: for a data file of a certain target point position, there are several sets of sample data, if the anchor point ranging value measured by a certain set of sample data is empty/0, it is considered as This set of data is missing data.

(k、k’＝1，2，3，...，n，且k≠k’)，则视为第k组样本数据与第k’组样本数据为相同数据。As an optimization, the definition of the same data is: for the data file of a certain target point position, there are several sets of sample data, and a set of sample data is used as the research object, if the anchor point of the kth set of data is the same as that of the target point The distance is the same as that of the k'th group of data, namely

(k, k'=1, 2, 3, . . . , n, and k≠k'), it is considered that the sample data of the kth group and the sample data of the k'th group are the same data.

The mathematical model of useless data can be expressed as:

为平均值；

为标准差。In the formula:

is the average value;

is the standard deviation.

S5. Establish a positioning model, use the test target to correct data deviation, and improve the positioning accuracy. The positioning model is determined through the following sub-steps:

则有：S51. Using the Chan algorithm to calculate the initial estimated position of the target. Suppose the positioning coordinates T(x, y, z) of the test target point, the anchor point coordinate position A _i ( _xi , y _i , z _i ), and the distance d _i between the target point and the anchor point. Due to problems such as interference, temperature drift and capacitive coupling, UWB measurement technology usually causes ranging errors, that is, d _i ≠(xx _i ) ² +(yy _i ) ² +(zz _i ) ² . Suppose the spatial distance between the test target point and the anchor point is

Then there are:

For the spatial distance between a certain set of target points and anchor points, it can be expressed as:

设误差为ε＝(ε_i)_n×1，则有：As an optimization, it is required that the ranging and spatial distance errors of the target point be as small as possible. Assuming that the distance measurement error of each anchor point is Δd _i , then

Let the error be ε=(ε _i ) _n×1 , then:

故有

那么，有：As an option, usually

Therefore there

Then, there are:

ε≈2D(Δd) (7)

Δd＝[Δd₁ Δd₂ … Δd_n]。In the formula:

Δd=[Δd ₁ Δd ₂ . . . Δd _n ].

那么误差向量ε的协方差矩阵为：As an optimization, let σ be the error threshold, and the covariance matrix of the measurement error vector Δd is

Then the covariance matrix of the error vector ε is:

φ=E(εε ^T )=4DE(Δd(Δd) ^T )D=4DQD (8)

为使得误差最小，运用加权最小二乘法计算，计算公式为：As an optimization, let the initial estimated position of the target point be

In order to minimize the error, the weighted least square method is used for calculation, and the calculation formula is:

S52. Using the non-line-of-sight residual discrimination method to calculate the positioning residual of the initial estimated position, and then compare the positioning residual of the initial estimated position with a given threshold to obtain the target observation position.

As an option, if the positioning residual value of the initial estimated position is less than or equal to a given threshold, then the position is accurate enough and is called the observed position; otherwise, Taylor series algorithm needs to be used to iteratively solve the observed position.

与精确坐标(x₀,y₀,z₀)各分量的误差对应的误差向量为δ＝(Δx,Δy,Δz)^T。将空间距离表达式在

处利用Taylor公式进行一阶展开，有：As an optimization, set the initial estimated position of the target point

The error vector corresponding to the error of each component of the exact coordinates (x ₀ , y ₀ , z ₀ ) is δ=(Δx, Δy, Δz) ^T . Put the spatial distance expression in

Use the Taylor formula to carry out the first-order expansion, which is:

为靶点的初始估计位置与锚点的空间距离；d_i0为靶点的精确坐标与锚点的空间距离。In the formula:

is the spatial distance between the initial estimated position of the target point and the anchor point; d _i0 is the spatial distance between the precise coordinates of the target point and the anchor point.

通过加权最小二乘法得到误差向量的迭代式如下：As an option, the error can be expressed as ε=(ε _i ) _n×1 . make:

The iterative formula for obtaining the error vector by the weighted least square method is as follows:

In the formula: Q is the covariance matrix of the measurement error.

的值，将这个值与阈值

进行比较。若

小于阈值，则该位置足够精确，这时的(x,y,z)称之为观测位置；反之，则需要通过坐标迭代式

进行迭代计算，直到

小于阈值。As an optimization, according to the iterative formula of the error vector δ, we can get

value, compare this value with the threshold

Compare. like

is less than the threshold, the position is accurate enough, and the (x, y, z) at this time is called the observation position; otherwise, it is necessary to pass the coordinate iterative formula

Perform iterative calculations until

less than the threshold.

S53. Use the Kalman filter algorithm to remove ranging noise and interference noise to improve positioning accuracy, and finally obtain the predicted position of the target.

As an optimization, Kalman filtering is divided into a prediction process and an update process. The prediction process is as follows:

In the formula: X _k,k-1 represents the estimated value of the state matrix at time k; X _k-1 represents the estimated value of the state matrix at time k-1; F _k-1 is the state transition matrix from time k-1 to time k ; P _k-1 represents the state covariance matrix at time k-1; P _k,k-1 represents the state covariance matrix at time k; process noise w _k ~N(0,Q _k ).

更新过程如下：As an optimization, use the update iteration of the Kalman filter algorithm to obtain the predicted position

The update process is as follows:

观测噪声v_k～N(0,R_k)；K_k为卡尔曼增益；R_k为观测噪声矩阵。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 is the observation noise matrix.

As an optimization, an adaptive robust Kalman filter is proposed for abnormal situations. The idea is to modify the covariance R _k and Q _k on the basis of the Kalman filter algorithm. The correction process is as follows:

在不考虑非视距误差且测量环境状况较好时，有v_k～N(0,R_k)。然而，当靶点与锚点之间存在干扰时，测距会出现异常值，可认为

其中

为准确距离。从而，可以设置检验测量值是否为异常的条件为：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 is in good condition, v _k ∼ N(0,R _k ). However, when there is interference between the target point and the anchor point, there will be outliers in ranging, which can be considered as

in

for the exact distance. Thus, the conditions for checking whether the measured value is abnormal can be set as:

In the formula: c is the threshold;

The revised formula for R _k is as follows:

As an optimization, the improved Sage-Husa filter is used to estimate the covariance matrix of the system noise in real time:

In the formula: α _k =(1-b)/(1-b ^k+1 ), forgetting factor 0<b<1.

S6. According to the positioning model, any object in the current application environment can be positioned, and at the same time, the motion track can be monitored and reproduced according to the time sequence of the data collected by the positioning target.

Compared with the prior art, the present invention has the following beneficial effects:

The UWB-based three-dimensional positioning model of the present invention solves the signal interference problem of UWB wireless communication technology, filters out useless data, reduces computing time and workload, and improves data accuracy. Combining Chan algorithm, Taylor series positioning algorithm and Kalman filter method to locate the target position, the positioning accuracy is improved. The trajectory of the target point can also be reproduced according to the time sequence of the collected data. This method improves the accuracy of the positioning model while reducing the calculation time, and can well solve the problems of indoor object positioning and moving track monitoring.

Description of drawings

In order to illustrate the technical solutions of the embodiments of the present invention more clearly, the accompanying drawings of the embodiments will be briefly introduced below. Obviously, the accompanying drawings in the following description only relate to some embodiments of the present invention, rather than limiting the present invention .

Fig. 1 is a schematic diagram of an embodiment of an anchor point arrangement in a test environment.

Fig. 2 is a schematic flow diagram of a positioning model of an embodiment.

Labels in the figure:

1-Test target point, 2-UWB anchor point.

Detailed ways

The present invention will be further described below in conjunction with the accompanying drawings and embodiments. It should be noted that, unless otherwise specified, all technical and scientific terms used in this application have the same meaning as commonly understood by those of ordinary skill in the art to which this application belongs.

In the present invention, the terms "first", "second" and the like are used to distinguish similar objects, rather than to describe a specific order or sequence, unless stated otherwise. The terms used should be understood as such; use of the terms "upper", "lower", "left", "right", etc. generally refers to the orientation shown in the drawings, or to the parts themselves in vertical, perpendicular or In terms of the direction of gravity; similarly, for the convenience of understanding and description, "inner", "outer", etc. refer to the inner and outer relative to the outline of each component itself. But the above location words are not used to limit the present invention.

A kind of UWB-based three-dimensional positioning model provided by the invention comprises the following steps:

S10, setting a reference point in the application environment, and based on this point, the site space is three-dimensionalized.

In a specific embodiment, the reference point and the three-dimensional coordinate system are as shown in FIG. 1 .

S20. Set n UWB anchor points at the edge, and the UWB anchor points transmit signals to all directions around.

In a specific embodiment, as shown in FIG. 1 . Four UWB anchor points are set in the application environment.

S30, setting a test target point, the target point can receive the signal transmitted from the anchor point and reflect it to the anchor point.

As an optimization, UWB adopts time-of-flight (TOF) ranging, and the distance between the anchor point and the target point can be obtained according to the time difference between the anchor point transmitting and receiving signals and the signal propagation speed, and 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, which is 3×10 ⁵ km/s; Δt is the time difference between sending and receiving signals of the anchor point, s.

In a specific embodiment, the sample data collected by a certain group is expressed 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: Timestamp: Abbreviation of Range Report: Tag ID: Anchor point ID: Distance measurement value of the anchor point (mm): Check value: Data serial number: Data number)

(i＝1，2，3，...，n；k＝1，2，3，...，m，n为锚点个数，m为样本组数)。S40. Data classification and cleaning, eliminating useless data, improving data accuracy, and reducing computing time and workload. The anchor point and the target point will send and receive signals every 0.2-0.3 seconds. At the same point, UWB will collect multiple sets of data. The longer the target point stays at the same position, the more the number of sets will be. many. Therefore, during the acquisition process, many useless (abnormal, missing, identical) data will be generated due to problems such as the residence time of the target, obstacles, and signal interference. Record the distance between the target point and the anchor point as

(i=1, 2, 3, . . . , n; k=1, 2, 3, . . . , m, n is the number of anchor points, and m is the number of sample groups).

As an optimization, the definition of abnormal data is: for the data file of a certain target point, there are several sets of sample data. According to the 3σ rule, if the deviation between a certain sample data and the average value exceeds two standard deviations, it is called This set of data is abnormal data. When the deviation exceeds three standard deviations, it is called a highly abnormal outlier.

As an optimization, the definition of missing data is: for a data file of a certain target point position, there are several sets of sample data, if the anchor point ranging value measured by a certain set of sample data is empty/0, it is considered as This set of data is missing data.

(k、k’＝1，2，3，...，n，且k≠k’)，则视为第k组样本数据与第k’组样本数据为相同数据。As an optimization, the definition of the same data is: for the data file of a certain target point position, there are several sets of sample data, and a set of sample data is used as the research object, if the anchor point of the kth set of data is the same as that of the target point The distance is the same as that of the k'th group of data, namely

(k, k'=1, 2, 3, . . . , n, and k≠k'), it is considered that the sample data of the kth group and the sample data of the k'th group are the same data.

The mathematical model of useless data can be expressed as:

为平均值；

为标准差。In the formula:

is the average value;

is the standard deviation.

S5. Establish a positioning model, use the test target to correct data deviation, and improve the positioning accuracy. The positioning model is determined through the following sub-steps:

S51. Using the Chan algorithm to calculate the initial estimated position of the target. Suppose the positioning coordinates T(x, y, z) of the test target point, the coordinate position A _i ( _xi , y _i , _zi ) of the anchor point, and the distance d _i between the target point to be tested and the anchor point. Due to problems such as interference, temperature drift and capacitive coupling, UWB measurement technology usually causes ranging errors, that is, d _i ≠(xx _i ) ² +(yy _i ) ² +(zz _i ) ² .

In a specific embodiment, as shown in FIG. 1 . The coordinates of the four UWB anchor points are: A0 (0, 0, 1300), A1 (5000, 0, 1700), A2 (0, 5000, 1700), A3 (5000, 5000, 1300).

可表示为：As an optimization, it is assumed that the spatial distance between the target point and the anchor point is

Can be expressed as:

In a specific embodiment, the spatial distance between the target point and the anchor point can be expressed as:

Suppose X _α =[xyz R] ^T , then it can be obtained from the above formula:

G _α X _α =H _α (5)

In the formula:

设误差为ε＝(ε_i)_n×1，则有：As an optimization, the ranging and spatial distance from the target point to each anchor point should be as small as possible, that is, min||(xx _i ) ² +(yy _i ) ² +(zz _i ) ² -d _i ||. Assuming that the distance measurement error of each anchor point is Δd _i , then

Let the error be ε=(ε _i ) _n×1 , then:

故有

那么，有：As an option, usually

Therefore there

Then, there are:

ε≈2D(Δd) (7)

Δd＝[Δd₁ Δd₂ Δd₃ Δd₄]^T。In the formula:

Δd=[Δd ₁ Δd ₂ Δd ₃ Δd ₄ ] ^T .

那么误差向量ε的协方差矩阵为：As an optimization, let σ be the error threshold, and the covariance matrix of the measurement error vector Δd is

Then the covariance matrix of the error vector ε is:

φ=E(εε ^T )=4DE(Δd(Δd) ^T )D=4DQD (8)

为使得误差最小，运用加权最小二乘法计算，计算式为：As an optimization, let the initial estimated position of the target point be

In order to minimize the error, the weighted least square method is used to calculate, and the calculation formula is:

S52. Using the non-line-of-sight residual discrimination method to calculate the positioning residual of the initial estimated position, and then compare the positioning residual of the initial estimated position with a given threshold to obtain the target observation position.

As an option, if the positioning residual value of the initial estimated position is less than or equal to a given threshold, the position is accurate enough and is called the observed position; otherwise, Taylor series positioning algorithm needs to be used to iteratively solve the observed position.

与精确坐标(x₀,y₀,z₀)各分量的误差对应的误差向量为δ＝(Δx,Δy,Δz)^T。将空间距离表达式在

处利用Taylor公式进行一阶展开，有：As an optimization, set the initial estimated position of the target point

The error vector corresponding to the error of each component of the exact coordinates (x ₀ , y ₀ , z ₀ ) is δ=(Δx, Δy, Δz) ^T . Put the spatial distance expression in

Use the Taylor formula to carry out the first-order expansion, which is:

为测试靶点的初始估计位置与锚点的空间距离；d_i0为测试靶点的精确坐标与锚点的空间距离；

In the formula:

is the spatial distance between the initial estimated position of the test target and the anchor point; d _i0 is the spatial distance between the precise coordinates of the test target and the anchor point;

通过加权最小二乘法得到误差向量的迭代式如下：As an option, the error can be expressed as ε=(ε _i ) _n×1 . make:

The iterative formula for obtaining the error vector by the weighted least square method is as follows:

In the formula: Q is the covariance matrix of the measurement error.

的值，将这个值与阈值

进行比较。若

小于阈值，则该位置足够精确，这时的(x,y,z)称之为观测位置；反之，则需要通过坐标迭代式

进行迭代计算，直到

小于阈值。As an optimization, according to the iterative formula of the error vector δ, we can get

value, compare this value with the threshold

Compare. like

is less than the threshold, the position is accurate enough, and the (x, y, z) at this time is called the observation position; otherwise, it is necessary to pass the coordinate iterative formula

Perform iterative calculations until

less than the threshold.

S53. Use the Kalman filter algorithm to remove ranging noise and interference noise to improve positioning accuracy, and finally obtain the predicted position of the target.

As an optimization, Kalman filtering is divided into a prediction process and an update process. The prediction process is as follows:

表示在k时刻状态矩阵的估计值；

表示k-1时刻状态矩阵的估计值为Taylor法计算得到的坐标(x,y,z)；F_k-1为k-1时刻到k时刻的状态转移矩阵，F_k-1＝I₃；P_k-1表示k-1时刻的状态协方差矩阵；P_k,k-1表示k时刻的状态协方差矩阵，P₀＝I₃；过程噪声w_k～N(0,Q_k)，

In the formula:

Indicates the estimated value of the state matrix at time k;

Representing the estimated value of the state matrix at k-1 time is the coordinates (x, y, z) calculated by Taylor method; F _k-1 is the state transition matrix from k-1 time to k time, F _k-1 =I ₃ ; P _k-1 represents the state covariance matrix at time k-1; P _k,k-1 represents the state covariance matrix at time k, P ₀ =I ₃ ; process noise w _k ～N(0,Q _k ),

更新过程如下：As an optimization, use the update iteration of the Kalman filter algorithm to obtain the predicted position

The update process is as follows:

观测噪声v_k～N(0,R_k)；卡尔曼增益为K_k，其大小表示更相信预测值

还是观测值

观测噪声矩阵

In the formula: observation vector

Observation noise v _k ～N(0,R _k ); Kalman gain is K _k , and its size indicates more confidence in the predicted value

or observations

observation noise matrix

As an optimization, an adaptive robust Kalman filter is proposed for abnormal situations. The idea is to modify the covariance R _k and Q _k on the basis of the Kalman filter algorithm. The correction process is as follows:

协方差矩阵为

在不考虑非视距误差且测量环境状况较好时，有v_k～N(0,R_k)。然而，当靶点与锚点之间存在干扰时，测距会出现异常值，可认为

其中

为准确距离。从而，可以设置检验测量值是否为异常的条件为：observation noise

The covariance matrix is

When the non-line-of-sight error is not considered and the measurement environment is in good condition, v _k ∼ N(0,R _k ). However, when there is interference between the target point and the anchor point, there will be outliers in ranging, which can be considered as

in

for the exact distance. Thus, the conditions for checking whether the measured value is abnormal can be set as:

In the formula: c is the threshold;

The revised formula for R _k is as follows:

As an optimization, the improved Sage-Husa filter is used to estimate the covariance matrix of the system noise in real time:

In the formula: α _k =(1-b)/(1-b ^k+1 ), forgetting factor b=0.5.

In a specific embodiment, the precise coordinates of the test target point are (450, 450, 200), and the obtained predicted position is (453, 461, 219), and the coordinate error is within 20 mm, which has high accuracy.

S60. According to the positioning model, any object in the current application environment can be positioned, and at the same time, the movement track can be monitored and reproduced according to the time sequence of the data collected by the positioning target.

In a specific embodiment, the positioning model is used to predict the position of the target point, and the predicted coordinates of the target point are obtained as shown in Table 1.

Table 1 Target prediction coordinates and dimensional accuracy of a certain embodiment

Compared with the prior art, the present invention has the following beneficial effects:

The UWB-based three-dimensional positioning model of the present invention can well solve the signal interference problem of the UWB wireless communication technology, and the positioning model is corrected by using test targets, thereby improving the positioning accuracy. The scene space is three-dimensionalized, and the position monitoring and moving track reproduction of any object can be performed according to the coordinate position and acquisition time of the target object. During the specific implementation, the model correction can be performed on the application scene through the anchor point and the test target point, which can be well extended and applied to different indoor environments, and has high accuracy.

The above description is only a preferred embodiment of the present invention, and does not limit the present invention in any form. Although the present invention has been disclosed as above with preferred embodiments, it is not intended to limit the present invention. Anyone familiar with this field Those skilled in the art, without departing from the scope of the technical solution of the present invention, can use the technical content disclosed above to make some changes or modify equivalent embodiments with equivalent changes, but all the content that does not depart from the technical solution of the present invention, according to the present invention Any simple modifications, equivalent changes and modifications made to the above embodiments by the technical essence still belong to the scope of the technical solutions of the present invention.

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 ⁵ 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 ² +y ² +z ² ；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 ₁ Δd ₂ …Δ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) ₀ ,y ₀ ,z ₀ ) 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.

Images