CN110493742B - An indoor three-dimensional localization method for ultra-wideband - Google Patents

Abstract

The invention discloses an indoor three-dimensional positioning method for ultra-wideband, and belongs to the field of communication. The invention includes: establishing a three-dimensional rectangular coordinate system in space; using a non-line-of-sight error discrimination method based on TDOA measurement distance distribution to determine whether there is a non-line-of-sight error in a positioning process; using an ultra-wideband system to make a preliminary three-dimensional position of the tag through the TDOA measurement data Position estimation: The three-dimensional position of the label estimated by the Chan algorithm is used for residual value weighting processing; the processed three-dimensional position of the label is used as the initial value of the Gauss-Newton iterative algorithm. 3D space location. The present invention provides a specific positioning scheme for using the TDOA positioning method; compared with the Chan algorithm, the present invention has better positioning accuracy than the Chan algorithm; the present invention has good anti-indoor non-line-of-sight error interference performance.

Description

An indoor three-dimensional localization method for ultra-wideband

technical field

The invention belongs to the field of communications, and in particular relates to an indoor three-dimensional positioning method for ultra-wideband.

Background technique

With the development of the economy, the expansion of commercial activities towards indoorization, and the high requirements of automation and intelligence in industrial fields such as smart factories and smart warehousing, indoor positioning has gradually attracted extensive attention and research by scholars at home and abroad.

The indoor positioning technology used for UWB technology can be summarized into four categories: based on signal time of arrival (TOA), based on signal time difference of arrival (TDOA), based on signal attenuation strength (RSSI), based on signal angle of arrival (AOA) ). Due to the non-line-of-sight propagation effect, multipath effect and multiple access interference of radio waves in the indoor environment, the TOA/TDOA technology is mostly used in the positioning method for ultra-wideband, and the TOA positioning technology requires strict time synchronization, hardware and practical It is difficult to realize in the scene, so domestic and foreign scholars widely use the TODA method. The nonlinear equation system established by the measurement values required for positioning obtained by TDOA usually needs to be transformed into a linear equation system for solving. The earliest scholars used the least squares method to study the TDOA positioning method, but the solution value was not the optimal solution. The Gauss-Newton iterative algorithm can obtain more accurate solution results, with fast convergence speed and strong robustness, which is very suitable for solving nonlinear equations. However, it has a strong dependence on the initial value of the iterative operation, and it needs to meet a certain accuracy of the initial value to obtain a high convergence speed. When the Gauss-Newton iterative algorithm is used, the adjacent coordinates that are close to the real coordinates to be obtained are selected as the initial values of the iterative operation, which can effectively prevent the divergence of the iterative operation. However, the Chan algorithm has high accuracy in the line-of-sight environment and does not require an initial value. It is suitable for the TDOA positioning method. The zero-mean Gaussian noise used in its error model is affected by the error interference in the non-line-of-sight environment. The accuracy will decrease .

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide an indoor three-dimensional positioning method for ultra-wideband, which can effectively adapt to the non-line-of-sight environment existing indoors.

The object of the present invention is achieved in this way:

An indoor three-dimensional positioning method for ultra-wideband, characterized in that it comprises the following steps:

Step 1: Distribute the ultra-wideband base stations in the indoor space, and establish a three-dimensional rectangular coordinate system in space;

Step 2: Use the non-line-of-sight error discrimination method based on the TDOA measurement distance distribution to determine whether there is a non-line-of-sight error in the positioning process;

Step 3: Derive the three-dimensional space Chan algorithm, and use the ultra-wideband system to estimate the three-dimensional position of the tag through the TDOA measurement data;

Step 4: The three-dimensional position of the label estimated by the Chan algorithm is weighted by the residual value;

Step 5: Take the processed three-dimensional position of the label as the initial value of the Gauss-Newton iterative algorithm, perform the iterative operation, and use the residual discriminant method to obtain the three-dimensional spatial position of the label.

The step 1 includes:

Establish a three-dimensional rectangular coordinate system in space; set the label coordinates (x, y, z), base station coordinates (X _i , Y _i , Z _i ), c is the propagation speed of electromagnetic waves in the air; t _i is the ultra-wideband signal from the label to the first. The time of i base stations, t _i,1 is the time difference between the UWB signal from the tag to the i-th base station and the first base station, t _i,1 =t _i -t ₁ ; ri is the tag to the _i -th base station The true spatial distance of , _{ri = ct i ,} and is expressed as:

r _i,1 is the distance difference between the tag and the ith base station and the first base station r _i,1 =r _i -r ₁ , expressed as:

The step 2 includes:

Let t _i be the time from the tag to the i-th base station of the UWB signal, t _i,1 be the time difference between the UWB signal from the tag to the i-th base station and the reference station, t _i,1 =t _i -t ₁ ; c is The propagation speed of electromagnetic waves in the air; the TDOA measurement value obtained by the i-th base station is Equation (3). Ideally, t _i,1 is the real signal propagation time difference. In practice, signal propagation may be affected by NLOS error and UWB system error. , the specific relationship is shown in formula (4), where t _i,1LOS is the time difference of signal propagation under LOS, t _e is the system measurement error, and t _i,1NLOS is the delay error caused by the NLOS factor;

R(t _i,1 )=c*t _i,1 (3)

t _i,1 =t _i,1LOS +t _i,1NLOS +t _e (4)

Bring equation (4) into equation (3) and quantify it as the distance difference of TDOA method, and obtain equation (5), where LOS(t _i,1 ) is the real distance difference under the line of sight, NLOS(t _i,1 ) is the error distance of NLOS quantization, and R(t _e ) is the systematic error distance;

R(t _i,1 )=LOS(t _i,1 )+NLOS(t _i,1 )+R(t _e ) (5)

If the UWB system is under normal working conditions, the system error distance R(t _e ) is approximately considered to obey a normal distribution with a mean value of zero and a variance of ^σ2 ; The LOS(t _i,1 ) distance is determined as the distance is constant, so under the line-of-sight propagation, the TDOA measurement value approximately obeys a normal distribution with a mean of l and a variance of σ ² , as shown in equation (6); and NLOS The error is affected by the actual environment and obeys log-normal distribution or uniform distribution; by measuring whether the distance distribution approximately obeys the normal distribution, it can be judged whether there is NLOS error;

LOS(t _i,1 )+N(t _e )～N(l,σ ² ) (6)

The Jarque-Bera test in statistics is used to test the normality of the TDOA distance obtained at the minimum resolution of the UWB receiver to test whether the sample obeys the normal distribution; since the skewness of the normal distribution is zero and the kurtosis is 3, Therefore, the Jarque-Bera test constructs the test statistic by testing the skewness and kurtosis of the sample, which is:

In the formula, S is the sample skewness, and K is the sample kurtosis; when the sample size n is large enough, the statistic of formula (7) approximately obeys the chi-square distribution with 2 degrees of freedom; in this way, the TDOA measurement value obtained by the UWB system is detected. Whether it obeys the normal distribution, and then determines whether there is NLOS error interference in the positioning process.

Described step 3 includes the following steps:

推得

消除高次项得：Step 3.1: Derive the three-dimensional Chan algorithm, obtain a preliminary estimate of the label's three-dimensional space position through the Chan algorithm, according to

push

Eliminate higher-order terms to get:

移项并以z_a作为未知数建立线性方程得：in the formula

Shifting the term and establishing _a linear equation with za as the unknown, we get:

h=G _a z _a (9)

where:

为z_a真实值，即标签的真实值，设系统产生的信号时延随机误差n，均值为零，方差为σ，协方差阵为

TDOA量测值

为r_i真实值，则误差矢量方程及解析式为：Step 3.2: Set up

is the true value of za, that is, the true value of the label. Let the random error of the signal delay generated by the system _n , the mean value is zero, the variance is σ, and the covariance matrix is

TDOA measurement value

is the true value of _ri , then the error vector equation and analytical formula are:

利用式(11)求得误差协方差阵Ψ＝E(ψψ^T)＝c²BQB，设z_a的元素相互独立，对z_a做初步加权最小二乘估计，得：A known

Using the formula (11), the error covariance matrix Ψ=E(ψψ ^T ) _{= c} ² BQB is obtained, and the elements of za are independent of each other, and the preliminary weighted least squares estimation for za is obtained:

此时式(12)近似为：When the tag is far away from all base stations, it is approximately considered that the distance between the tag and each base station is equal to the distance between the tag and the ith base station, then B≈r _range I, r _range represents the distance, and I is the M-1 order unit matrix , then the error covariance matrix

At this time, formula (12) is approximated as:

Step 3.3: In practice, the element r ₁ in za is associated with ( _x , y, z), and there is enough noise e to make za _a random vector whose mean is the true value, then _Ga , h with _za expressed as:

From formula (9), we get:

Let e _i , _i = 1, 2, 3, 4 be noise, and the elements of za are expressed as:

za ₌ [x ⁰ +e ₁ , y ⁰ +e ₂ , z ⁰ +e ₃ , r ₁ ⁰ +e ₄ ] ^T (18)

The new linear equation is established as:

ψ'=h'-G _a 'z _a ' (19)

in:

Find _a weighted least squares estimate for za ':

In the formula, the Ψ' error covariance matrix is approximated by ignoring the high-order term in the calculation of Ψ' when e _i is small:

Ψ'=E[ ^ψ'ψ'T ]=4B'cov(z _a )B' (21)

B'=diag{x ⁰ -X ₁ ,y ⁰ -Y ₁ ,z ⁰ -Z ₁ ,r ₁ ⁰ } (22)

用G_a近似替代，而B'中所含的标签未知的真实坐标用式(12)，估计所得

近似计算，根据先验信息取舍，最终估计出标签坐标z_t为：cov(z _a ) contains unknown truth value

Approximately replace with _Ga , and use formula (12) for the unknown real coordinates of labels contained in B', and the estimated

Approximate calculation, according to the priori information, the final estimated label coordinate z _t is:

Described step 4 includes:

种组合，第k种组合记为S_k，表示该种组合所对应基站的集合，在该组合下利用Chan算法估算得到的标签坐标记为z_t,k＝[x_k,y_k,z_k]^T,k＝1,2,...,N，第i个基站的坐标记为A_i＝[X_i,Y_i,Z_i]，设对应的平方残值函数为：When the UWB system performs three-dimensional positioning, one of the base stations is selected as the reference station, and the remaining M-1 base stations are grouped. Due to the three-dimensional positioning, the number of base stations participating is at least 4, so the group has a total of

The k-th combination is denoted as S _k , which represents the set of base stations corresponding to this combination, and the label coordinates estimated by the Chan algorithm under this combination are denoted as z _t,k =[x _k ,y _k ,z _k ] ^T ,k=1,2,...,N, the coordinates of the ith base station are marked as A _i =[X _i ,Y _i ,Z _i ], and the corresponding square residual value function is set as:

In the formula,

Set each combination to use the Chan algorithm to estimate the position to give the weight w _k , w _k =1/p(z _t,k ,S _k ), then the label position is estimated as:

z _rw is the estimated position of the label in three-dimensional space. This method is a combination of the traditional Chan algorithm and the residual value weighting method.

Described step 5 includes the following steps:

Step 5.1: Set the applicable coordinate parameters, and establish the prototype of the Gauss-Newton iteration method corresponding to the three-dimensional positioning;

Let the real coordinates of the tag be (x, y, z), the coordinates of the i-th base station be (X _i , Y _i , Z _i ), and let m _k,i be the k-th TDOA measurement value measured by the i-th base station , that is, m _k,i =r _k,i -r _k,1 , i≥2 to establish the function model as:

f _k (x,y,z,X _i ,Y _i ,Z _i )=m _k,i -e _k ,k=1,2,...,n (26)

where e _k is the random variable error of statistical distribution, the mean is zero, the error covariance matrix _Re = [c _ij ], and the i-th row and j-column element c _ij =(e _ij ) in the matrix;

Step 5.2: The relationship between the label coordinate estimated value z _rw (x _v , y _v , z _v ) and the label real coordinate (x, y, z) after weighting by the square residual value is:

x=x _v +δ _x ,y=y _v +δ _y ,z=z _v +δ _z (27)

Expand the function prototype f _k Taylor series, keeping the first-order terms:

a _k1 δ _x +a _k2 δ _y +a _k3 δ _z =m _i,k -f _kv -e _k (28)

where:

Define matrices and vectors:

Formula (28) establishes a new equation as:

L=Aδ+e (29)

δ is estimated by doing weighted least squares as:

δ=[A ^T R _e ^-1 A] ^-1 A ^T R _e ^-1 L (30)

In the formula, the _Re matrix cannot be estimated because the prior information of the TDOA measurement value is unknown, and the _Re matrix can effectively suppress the NLOS error interference. The residual value square is used to select S _M-1 containing M-1 base station combinations, and the weighted error coefficient is constructed. The variance _matrix We is:

_Bringing the constructed weighted error covariance matrix We into equation (30), the least squares solution of δ is obtained as:

δ=[A ^T W _e ^-1 A] ^-1 A ^T W _e ^-1 L (31)

The iterated z _rw '(x _v ',y _v ',z _v ') is:

x _v '=x _v +δ _x ,y _v '=y _v +δ _y ,z _v '=z _v +δ _z (32)

The Gauss-Newton algorithm is used to perform continuous and repeated iterative operations. When |δ _x |+|δ _y |+|δ _z |<ε, and ε is sufficiently small, the Gauss-Newton iteration converges and the three-dimensional space position of the label is obtained;

Step 5.3: Establish a function model f(x), as shown in formula (33), where _ui is the TDOA value calculated by the corresponding label, and _mi is the original TDOA value obtained by the system; the coordinate corresponding to the minimum function value is the optimal estimated coordinate of the label;

Compared with the prior art, the beneficial effects of the present invention are:

(1) The present invention provides an indoor three-dimensional positioning method for ultra-wideband;

(2) The present invention provides a specific positioning scheme for using the TDOA positioning method;

(3) The present invention proposes and uses a non-line-of-sight error discrimination method based on TDOA measurement distance distribution;

(4) Compared with the Chan algorithm, the positioning accuracy of the present invention is better than that of the Chan algorithm;

(5) The present invention has good anti-indoor non-line-of-sight error interference performance.

Description of drawings

Fig. 1 is the overall block diagram of the indoor three-dimensional positioning method for ultra-wideband of the present invention;

Figure 2 is a schematic diagram of the three-dimensional spatial positioning of the TDOA method;

Figure 3 is a schematic diagram of an indoor three-dimensional space rectangular coordinate system;

Figure 4 is a flowchart of the three-dimensional space Chan algorithm;

Figure 5 is a flow chart of the Gauss-Newton iteration algorithm.

Detailed ways

The invention proposes an indoor three-dimensional positioning method for ultra-wideband. First, more than 4 ultra-wideband base stations are arranged for indoor scenes, and measurement data is obtained by TDOA. It is proposed to use the non-line-of-sight error discrimination method to determine whether the positioning process is a non-line-of-sight environment; use the Chan algorithm to perform two weighted least squares estimation on the TDOA original data obtained by the UWB system, and obtain the estimated value of the three-dimensional position of the tag; use the residual value. The weighting method smoothes the non-line-of-sight error generated by the application of the UWB system, and obtains a further accurate 3D position estimate of the label; the obtained estimated value is used as the initial value of Gauss-Newton iteration, multiple iterations are performed, and finally the residual discriminant is used. method to output the 3D space position of the label. The invention can effectively adapt to the non-line-of-sight environment existing indoors, and provides a reasonable and high-precision positioning method for indoor three-dimensional positioning for ultra-wideband.

The present invention will be described in detail below with reference to the accompanying drawings.

An indoor three-dimensional positioning method for ultra-wideband, as shown in Figure 1, includes the following steps:

Step 1: Distribute the ultra-wideband base stations in the indoor space according to the principle of TDOA positioning method, as shown in Figure 2, and establish a three-dimensional rectangular coordinate system in space, as shown in Figure 3.

Establish a three-dimensional rectangular coordinate system in space. Let the label coordinates (x, y, z), the base station coordinates (X _i , Y _i , Z _i ), and c be the propagation speed of electromagnetic waves in the air. t _i is the time from the UWB signal from the tag to the ith base station, t _i,1 is the time difference between the UWB signal from the tag to the ith base station and the first base station, t _i,1 =t _i -t ₁ . ri is the real spatial distance from the tag to the _ith base station, _{ri =ct i ,} and is expressed as

r _i,1 is the distance difference between the tag and the ith base station and the first base station r _i,1 =r _i -r ₁ , expressed as

Step 2: Use the non-line-of-sight error discrimination method based on the TDOA measurement distance distribution to determine whether there is a non-line-of-sight error in the positioning process.

Let t _i be the time from the tag to the ith base station of the UWB signal, t _i,1 be the time difference between the UWB signal from the tag to the ith base station and the reference station, t _i,1 =t _i -t ₁ . c is the propagation speed of electromagnetic waves in the air. The TDOA measurement value obtained by the i-th base station is Equation (3). Ideally, t _i,1 is the real signal propagation time difference. In practice, the signal propagation may be affected by NLOS error and UWB system error. The specific relationship is shown in Equation (4). ), where t _i,1LOS is the time difference of signal propagation under LOS, t _e is the system measurement error, and t _i,1NLOS is the delay error caused by the NLOS factor.

R(t _i,1 )=c*t _i,1 (3)

t _i,1 =t _i,1LOS +t _i,1NLOS +t _e (4)

Putting Equation (4) into Equation (3) can be quantified as the distance difference of the TDOA method, and then Equation (5) is obtained, where LOS(t _i,1 ) is the real distance difference under the line of sight, NLOS(t _{i,1 )} ) is the NLOS quantized error distance, and R(t _e ) is the systematic error distance.

R(t _i,1 )=LOS(t _i,1 )+NLOS(t _i,1 )+R(t _e ) (5)

If the UWB system works normally, its system error distance R(t _e ) can be approximately considered to obey the normal distribution with zero mean and variance σ ² . Since the data refresh rate of the UWB system can reach more than 100Hz, the receiver judges the LOS(t _i,1 ) distance as the same distance. Therefore, under line-of-sight propagation, the TDOA measurement value approximately obeys the mean value of l and the variance is σ ² The normal distribution of , as shown in Equation (6). The NLOS error is affected by the actual environment and may obey a log-normal distribution or a uniform distribution. By measuring whether the distance distribution approximately obeys the normal distribution, it can be judged whether there is NLOS error.

LOS(t _i,1 )+N(t _e )～N(l,σ ² ) (6)

The Jarque-Bera test in statistics is used to test the normality of the TDOA distance obtained at the minimum resolution of the UWB receiver to test whether the sample obeys the normal distribution. Since the skewness of the normal distribution is zero and the kurtosis is 3, the Jarque-Bera test constructs the test statistic by testing the skewness and kurtosis of the sample, which is

where S is the sample skewness and K is the sample kurtosis. When the sample size n is large enough, the statistic in equation (7) approximately obeys the chi-square distribution with 2 degrees of freedom. In this way, it is detected whether the TDOA measurement value obtained by the UWB system obeys the normal distribution, and then it is determined whether there is NLOS error interference in the positioning process.

Step 3: Derive the three-dimensional space Chan algorithm, and use the ultra-wideband system to estimate the three-dimensional position of the tag through the TDOA measurement data. The process is shown in Figure 4.

可推得

带入式(1)，消除高次项可得Step 3.1: Derive a suitable three-dimensional Chan algorithm, and obtain a preliminary estimate of the label's three-dimensional space position through the Chan algorithm. according to

can be pushed

Bringing into equation (1), eliminating the high-order term can get

移项并以z_a作为未知数建立线性方程可得in the formula

By shifting the term and establishing _a linear equation with za as the unknown, we get

h=G _a z _a (9)

in the formula

为z_a真实值，即标签的真实值，假设系统产生的信号时延随机误差n，均值为零，方差为σ，协方差阵为

TDOA量测值

为r_i真实值，则误差矢量方程及解析式为：Step 3.2: Set up

is the true value of za, that is, the true value of the label. Assuming that the random error of the signal delay generated by the system is _n , the mean is zero, the variance is σ, and the covariance matrix is

TDOA measurement value

is the true value of _ri , then the error vector equation and analytical formula are:

利用式(11)求得误差协方差阵Ψ＝E(ψψ^T)＝c²BQB，假设z_a的元素相互独立，对z_a做初步加权最小二乘估计，可得A known

Using the formula (11), the error covariance matrix Ψ=E(ψψ ^T ) _{= c} ² BQB can be obtained. Assuming that the elements of za are independent of each other, the preliminary weighted least squares estimation of za can be obtained.

此时式(12)近似为When the tag is far away from all base stations, it is approximately considered that the distance between the tag and each base station is equal to the distance between the tag and the ith base station, then B≈r _range I, r _range represents the distance, and I is the M-1 order unit matrix , then the error covariance matrix

At this time, equation (12) is approximated as

Step 3.3: In practice, the element r ₁ in za is associated with ( _x , y, z), assuming that there is enough noise e, so that za becomes _a random vector whose mean is the true value, then _Ga , h with _za denoted as

Putting equation (9) into equation (5), it can be obtained in turn:

Let e _i , _i = 1, 2, 3, 4 be noise, and the elements of za are expressed as

za ₌ [x ⁰ +e ₁ , y ⁰ +e ₂ , z ⁰ +e ₃ , r ₁ ⁰ +e ₄ ] ^T (18)

The new linear equation can be established as

ψ'=h'-G _a 'z _a ' (19)

in:

find _a weighted least squares estimate of za '

In the formula, the Ψ' error covariance matrix is approximated by ignoring the high-order term in the calculation of Ψ' when e _i is small:

Ψ'=E[ ^ψ'ψ'T ]=4B'cov(z _a )B' (21)

B'=diag{x ⁰ -X ₁ ,y ⁰ -Y ₁ ,z ⁰ -Z ₁ ,r ₁ ⁰ } (22)

用G_a近似替代，而B'中所含的标签未知的真实坐标用式(12)，估计所得

近似计算，根据先验信息取舍，最终可估计出标签坐标z_t为cov(z _a ) contains unknown truth value

Approximately replace with _Ga , and use formula (12) for the unknown real coordinates of labels contained in B', and the estimated

Approximate calculation, according to the priori information, the label coordinate z _t can finally be estimated as

Step 4: The three-dimensional position of the label estimated by the Chan algorithm is weighted by the residual value;

种组合，第k种组合记为S_k，表示该种组合所对应基站的集合，在该组合下利用Chan算法估算得到的标签坐标记为z_t,k＝[x_k,y_k,z_k]^T,k＝1,2,...,N，第i个基站的坐标记为A_i＝[X_i,Y_i,Z_i]，设对应的平方残值函数为When the UWB system performs three-dimensional positioning, one of the base stations is selected as the base station, and the remaining M-1 base stations are grouped. Due to the three-dimensional positioning, the number of base stations participating is at least 4, so the group has a total of

The kth combination is denoted as S _k , which represents the set of base stations corresponding to this combination, and the label coordinates estimated by the Chan algorithm under this combination are denoted as z _t,k =[x _k ,y _k ,z _k ] ^T ,k=1,2,...,N, the coordinates of the i-th base station are A _i =[X _i ,Y _i ,Z _i ], and the corresponding square residual value function is

in the formula

Set each combination to use the Chan algorithm to estimate the position to give weight w _k , w _k =1/p(z _t,k ,S _k ), then the label position is estimated as

z _rw is the estimated position of the label in three-dimensional space. This method is a combination of the traditional Chan algorithm and the residual value weighting method.

Step 5: Use the processed three-dimensional position of the label as the initial value of the Gauss-Newton iterative algorithm, perform iterative operations, and use the residual discriminant method to obtain the three-dimensional spatial position of the label. The process is shown in Figure 5.

Step 5.1: Set the applicable coordinate parameters, and establish the Gauss-Newton iteration method function prototype corresponding to the three-dimensional positioning.

Let the real coordinates of the tag be (x, y, z), the coordinates of the i-th base station be (X _i , Y _i , Z _i ), and let m _k,i be the k-th TDOA measurement value measured by the i-th base station , that is, m _k,i =r _k,i -r _k,1 , i≥2 to establish a function model as

f _k (x,y,z,X _i ,Y _i ,Z _i )=m _k,i -e _k ,k=1,2,...,n (26)

In the formula, e _k is the random variable error of statistical distribution, the mean value is zero, the error covariance matrix _Re =[c _ij ], and the i-th row and j-column element c _ij =(e _ij ) in the matrix.

Step 5.2: The relationship between the label coordinate estimated value z _rw (x _v , y _v , z _v ) and the label real coordinate (x, y, z) after weighting by the square residual value is as follows

x=x _v +δ _x , y=y _v +δ _y , z=z _v +δ _z (27)

Expand the function prototype f _k Taylor series, keeping the first-order terms

a _k1 δ _x +a _k2 δ _y +a _k3 δ _z =m _i,k -f _kv -e _k (28)

in the formula

Defining Matrices and Vectors

Equation (28) can establish a new equation as

L=Aδ+e (29)

By doing weighted least squares estimation, δ can be obtained as

δ=[A ^T R _e ^-1 A] ^-1 A ^T R _e ^-1 L (30)

In the formula, the _Re matrix cannot be estimated because the prior information of the TDOA measurement value is unknown, and the _Re matrix can effectively suppress the NLOS error interference. The residual value square is used to select S _M-1 containing M-1 base station combinations, and the weighted error coefficient is constructed. The variance _{matrix We} is

Bring the constructed weighted error _covariance matrix We into equation (30), and obtain the least squares solution of δ as

δ=[A ^T W _e ^-1 A] ^-1 A ^T W _e ^-1 L (31)

The iterated z _rw '(x _v ',y _v ',z _v ') is

x _v '=x _v +δ _x ,y _v '=y _v +δ _y ,z _v '=z _v +δ _z (32)

The Gauss-Newton algorithm is used to perform continuous repeated iterative operations. When |δ _x |+|δ _y |+|δ _z |<ε, and ε is sufficiently small, the Gauss-Newton iteration converges and the three-dimensional space position of the label is obtained.

Step 5.3: Establish a function model f(x), as shown in formula (33), where _ui is the TDOA value calculated by the corresponding label, and _mi is the original TDOA value obtained by the system. The coordinate corresponding to the minimum function value is the optimal estimated coordinate of the label.

The content not described in detail in the present invention belongs to the known technology of those skilled in the art.

Claims (3)

1. an indoor three-dimensional positioning method for ultra-wideband, is characterized in that, comprises the following steps: Step 1: Distribute the ultra-wideband base stations in the indoor space, and establish a three-dimensional rectangular coordinate system in space; Step 2: Use the non-line-of-sight error discrimination method based on the TDOA measurement distance distribution to determine whether there is a non-line-of-sight error in the positioning process; Step 3: Derive the three-dimensional space Chan algorithm, and use the ultra-wideband system to estimate the three-dimensional position of the tag through the TDOA measurement data; Step 4: The three-dimensional position of the label estimated by the Chan algorithm is weighted by the residual value; Step 5: Take the processed three-dimensional position of the label as the initial value of the Gauss-Newton iterative algorithm, perform iterative operations, and use the residual discriminant method to obtain the three-dimensional space position of the label; the step 1 includes: Establish a three-dimensional rectangular coordinate system in space; set the label coordinates (x, y, z), base station coordinates (X _i , Y _i , Z _i ), c is the propagation speed of electromagnetic waves in the air; t _i is the ultra-wideband signal from the label to the first. The time of i base stations, t _i,1 is the time difference between the UWB signal from the tag to the i-th base station and the first base station, t _i,1 =t _i -t ₁ ; ri is the tag to the _i -th base station The true spatial distance of , _{ri = ct i ,} and is expressed as:

r _i,1 is the distance difference between the tag and the ith base station and the first base station r _i,1 =r _i -r ₁ , expressed as:

The step 2 includes: Let t _i be the time from the tag to the i-th base station of the UWB signal, t _i,1 be the time difference between the UWB signal from the tag to the i-th base station and the reference station, t _i,1 =t _i -t ₁ ; c is The propagation speed of electromagnetic waves in the air; the TDOA measurement value obtained by the i-th base station is Equation (3). Ideally, t _i,1 is the real signal propagation time difference. In practice, signal propagation may be affected by NLOS error and UWB system error. , the specific relationship is shown in formula (4), where t _i,1LOS is the time difference of signal propagation under LOS, t _e is the system measurement error, and t _i,1NLOS is the delay error caused by the NLOS factor; R(t _i,1 )=c*t _i,1 (3) t _i,1 =t _i,1LOS +t _i,1NLOS +t _e (4) Bring equation (4) into equation (3) and quantify it as the distance difference of TDOA method, and obtain equation (5), where LOS(t _i,1 ) is the real distance difference under the line of sight, NLOS(t _i,1 ) is the error distance of NLOS quantization, and R(t _e ) is the systematic error distance; R(t _i,1 )=LOS(t _i,1 )+NLOS(t _i,1 )+R(t _e ) (5) If the UWB system is under normal working conditions, the system error distance R(t _e ) is approximately considered to obey a normal distribution with a mean value of zero and a variance of ^σ2 ; The LOS(t _i,1 ) distance is determined as the distance is constant, so under the line-of-sight propagation, the TDOA measurement value approximately obeys a normal distribution with a mean of l and a variance of σ ² , as shown in equation (6); and NLOS The error is affected by the actual environment and obeys log-normal distribution or uniform distribution; by measuring whether the distance distribution approximately obeys the normal distribution, it can be judged whether there is NLOS error; LOS(t _i,1 )+N(t _e )～N(l,σ ² ) (6) The Jarque-Bera test in statistics is used to test the normality of the TDOA distance obtained at the minimum resolution of the UWB receiver to test whether the sample obeys the normal distribution; since the skewness of the normal distribution is zero and the kurtosis is 3, Therefore, the Jarque-Bera test constructs the test statistic by testing the skewness and kurtosis of the sample, which is:

In the formula, S is the sample skewness, and K is the sample kurtosis; when the sample size n is large enough, the statistic of formula (7) approximately obeys the chi-square distribution with 2 degrees of freedom; in this way, the TDOA measurement value obtained by the UWB system is detected. Whether it obeys the normal distribution, and then determines whether there is NLOS error interference in the positioning process; Described step 3 includes the following steps: Step 3.1: Derive the three-dimensional Chan algorithm, obtain a preliminary estimate of the label's three-dimensional space position through the Chan algorithm, according to

push

Eliminate higher-order terms to get:

in the formula

Shifting the term and establishing _a linear equation with za as the unknown, we get: h=G _a z _a (9) where:

Step 3.2: Set up

is the true value of za, that is, the true value of the label. Let the random error of the signal delay generated by the system _n , the mean value is zero, the variance is σ, and the covariance matrix is

TDOA measurement value

is the true value of _ri , then the error vector equation and analytical formula are:

A known

Using the formula (11), the error covariance matrix Ψ=E(ψψ ^T ) _{= c} ² BQB is obtained, and the elements of za are independent of each other, and the preliminary weighted least squares estimation for za is obtained:

When the tag is far away from all base stations, it is approximately considered that the distance between the tag and each base station is equal to the distance between the tag and the ith base station, then B≈r _range I, r _range represents the distance, and I is the M-1 order unit matrix , then the error covariance matrix

At this time, formula (12) is approximated as:

Step 3.3: In practice, the element r ₁ in za is associated with ( _x , y, z), and there is enough noise e to make za _a random vector whose mean is the true value, then _Ga , h with _za expressed as:

From formula (9), we get:

Let e _i , _i = 1, 2, 3, 4 be noise, and the elements of za are expressed as: za ₌ [x ⁰ +e ₁ , y ⁰ +e ₂ , z ⁰ +e ₃ , r ₁ ⁰ +e ₄ ] ^T (18) The new linear equation is established as: ψ'=h'-G _a 'z _a ' (19) in:

Find _a weighted least squares estimate for za ':

In the formula, the Ψ' error covariance matrix is approximated by ignoring the high-order term in the calculation of Ψ' when e _i is small: Ψ'=E[ψ'ψ' ^T ]=4B'cov(z _a )B' (21) B'=diag{x ⁰ -X ₁ ,y ⁰ -Y ₁ ,z ⁰ -Z ₁ ,r ₁ ⁰ } (22) cov(z _a ) contains unknown truth value

Approximately replace with _Ga , and use formula (12) for the unknown real coordinates of labels contained in B', and the estimated

Approximate calculation, according to the priori information, the final estimated label coordinate z _t is:

Among them, ri is the real spatial distance from the tag to the _ith base station; n _i,1 is the difference between the random error of the delay between the signal transmitted by the tag reaching the ith base station and the base station; x, y, z is the label coordinate, X _i , Y _i , Z _i are the base station coordinates; X _i,1 =X _i -X ₁ , that is, the difference between the X-axis coordinate of the i-th base station and the X-axis coordinate of the first base station; Y _i,1 =Y _i -Y ₁ , that is, the difference between the Y-axis coordinate of the i-th base station and the Y-axis coordinate of the first base station; Z _i,1 =Z _i -Z ₁ , that is, the difference between the Z-axis coordinate of the i-th base station and the Z-axis coordinate of the first base station. 2. a kind of indoor three-dimensional positioning method for ultra-wideband according to claim 1, is characterized in that, described step 4 comprises: When the UWB system performs three-dimensional positioning, one of the base stations is selected as the reference station, and the remaining M-1 base stations are grouped. Due to the three-dimensional positioning, the number of base stations participating is at least 4, so the group has a total of

The k-th combination is denoted as S _k , which represents the set of base stations corresponding to this combination, and the label coordinates estimated by the Chan algorithm under this combination are denoted as z _t,k =[x _k ,y _k ,z _k ] ^T ,k=1,2,...,N, the coordinates of the ith base station are marked as A _i =[X _i ,Y _i ,Z _i ], and the corresponding square residual value function is set as:

In the formula,

Set each combination to use the Chan algorithm to estimate the position to give the weight w _k , w _k =1/p(z _t,k ,S _k ), then the label position is estimated as:

z _rw is the estimated position of the label in three-dimensional space. This method is a combination of the traditional Chan algorithm and the residual value weighting method. 3. a kind of indoor three-dimensional positioning method for ultra-wideband according to claim 1, is characterized in that, described step 5 comprises the following steps: Step 5.1: Set the applicable coordinate parameters, and establish the prototype of the Gauss-Newton iteration method corresponding to the three-dimensional positioning; Let the real coordinates of the tag be (x, y, z), the coordinates of the i-th base station be (X _i , Y _i , Z _i ), and let m _k,i be the k-th TDOA measurement value measured by the i-th base station , that is, m _k,i =r _k,i -r _k,1 , i≥2 to establish the function model as: f _k (x,y,z,X _i ,Y _i ,Z _i )=m _k,i -e _k ,k=1,2,...,n (26) where e _k is the random variable error of statistical distribution, the mean is zero, the error covariance matrix _Re = [c _ij ], and the i-th row and j-column element c _ij =(e _ij ) in the matrix; Step 5.2: The relationship between the label coordinate estimated value z _rw (x _v , y _v , z _v ) and the label real coordinate (x, y, z) after weighting by the square residual value is: x=x _v +δ _x ,y=y _v +δ _y ,z=z _v +δ _z (27) Expand the function prototype f _k Taylor series, keeping the first-order terms: a _k1 δ _x +a _k2 δ _y +a _k3 δ _z =m _i,k -f _kv -e _k (28) where:

Define matrices and vectors:

Formula (28) establishes a new equation as: L=Aδ+e (29) By doing weighted least squares, δ is estimated as: δ=[A ^T R _e ^-1 A] ^-1 A ^T R _e ^-1 L (30) In the formula, the _Re matrix cannot be estimated because the prior information of the TDOA measurement value is unknown, and the _Re matrix can effectively suppress the NLOS error interference. The residual value square is used to select S _M-1 containing M-1 base station combinations, and the weighted error coefficient is constructed. The variance _matrix We is:

_Bringing the constructed weighted error covariance matrix We into equation (30), the least squares solution of δ is obtained as: δ=[A ^T W _e ^-1 A] ^-1 A ^T W _e ^-1 L (31) The iterated z _rw '(x _v ',y _v ',z _v ') is: x _v '=x _v +δ _x ,y _v '=y _v +δ _y ,z _v '=z _v +δ _z (32) The Gauss-Newton algorithm is used to perform continuous and repeated iterative operations. When |δ _x |+|δ _y |+|δ _z |<ε, and ε is sufficiently small, the Gauss-Newton iteration converges and the three-dimensional space position of the label is obtained; Step 5.3: Establish a function model f(x), as shown in formula (33), where _ui is the TDOA value calculated by the corresponding label, and _mi is the original TDOA value obtained by the system; the coordinate corresponding to the minimum function value is the optimal estimated coordinate of the label;

Images