[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN110933630A - Indoor three-dimensional positioning method and device based on ultra-wideband communication - Google Patents

Indoor three-dimensional positioning method and device based on ultra-wideband communication Download PDF

Info

Publication number
CN110933630A
CN110933630A CN201911182827.8A CN201911182827A CN110933630A CN 110933630 A CN110933630 A CN 110933630A CN 201911182827 A CN201911182827 A CN 201911182827A CN 110933630 A CN110933630 A CN 110933630A
Authority
CN
China
Prior art keywords
label
dimensional
tag
base station
estimated
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
Application number
CN201911182827.8A
Other languages
Chinese (zh)
Inventor
于虹
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Electric Power Research Institute of Yunnan Power System Ltd
Original Assignee
Electric Power Research Institute of Yunnan Power System Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Electric Power Research Institute of Yunnan Power System Ltd filed Critical Electric Power Research Institute of Yunnan Power System Ltd
Priority to CN201911182827.8A priority Critical patent/CN110933630A/en
Publication of CN110933630A publication Critical patent/CN110933630A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/10—Position of receiver fixed by co-ordinating a plurality of position lines defined by path-difference measurements, e.g. omega or decca systems
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention discloses an indoor three-dimensional positioning method and device based on ultra-wideband communication, the method obtains TDOA original data of a label, wherein, more than 4 ultra-wideband base stations are arranged in advance in an indoor scene where the label is positioned, a spatial three-dimensional rectangular coordinate system is established, TDOA original data of the label is obtained by the ultra-wideband base stations positioning the label in a TDOA mode, estimated three-dimensional position of the label is obtained by utilizing the TDOA original data and estimating through a three-dimensional Chan algorithm, the estimated three-dimensional position of the label is subjected to residual value weighting processing, the estimated three-dimensional position of the processed label is used as an initial value of a Gaussian-Newton iterative algorithm, the three-dimensional space position of the label is obtained through iterative operation, the label can effectively adapt to the indoor non-line-of-sight environment, and a reasonable and high-precision three-dimensional positioning scheme is provided for indoor three-dimensional positioning based on ultra-wideband communication.

Description

Indoor three-dimensional positioning method and device based on ultra-wideband communication
Technical Field
The invention relates to the technical field of communication, in particular to an indoor three-dimensional positioning method and device based on ultra-wideband communication.
Background
Along with the development of economy, the development of the indoor trend of commercial activities and the high requirements of automation and intellectualization in industrial fields such as intelligent factories and intelligent storage, the indoor positioning gradually draws the attention and research of domestic and foreign scholars. Indoor positioning technology based on ultra-wideband communication technology can be summarized into four broad categories: based on signal time of arrival (TOA), based on signal time difference of arrival (TDOA), based on signal strength of attenuation (RSSI), and based on angle of arrival (AOA). Because of the non-line-of-sight propagation effect, multipath effect and multiple access interference of electric waves in indoor environment, the positioning method based on ultra-wideband communication mostly adopts the TOA/TDOA technology, and the TOA positioning technology needs strict time synchronization and is difficult to realize in hardware and actual scenes, so that the TODA mode is widely adopted by scholars at home and abroad. The system of non-linear equations created from the measurements needed for TDOA location typically needs to be transformed into a system of linear equations to be solved.
The earliest learners studied the TDOA location using least squares, but the solution was not optimal. The Gaussian-Newton iterative algorithm can obtain a more accurate resolving result, has high convergence rate and strong robustness, and is very suitable for solving a nonlinear equation. However, the initial value of the iterative operation has strong dependency, and a higher convergence rate can be obtained only by satisfying the initial value with a certain accuracy. When the Gaussian-Newton iterative algorithm is used, the adjacent coordinate close to the real coordinate to be obtained is selected as the initial value of the iterative operation, so that the divergence of the iterative operation can be effectively prevented. The Chan algorithm has high precision in a line-of-sight environment, does not need an initial value, is suitable for a TDOA (time difference of arrival) positioning mode, and reduces the precision due to error interference in a non-line-of-sight environment due to zero mean Gaussian noise used by an error model.
Disclosure of Invention
The invention provides an indoor three-dimensional positioning method and device based on ultra-wideband communication, and aims to solve the problem that in the prior art, the precision is reduced due to the fact that the prior art is easily interfered by errors in a non-line-of-sight environment.
In a first aspect, the present invention provides an indoor three-dimensional positioning method based on ultra-wideband communication, the method comprising:
acquiring TDOA original data of a tag, wherein more than 4 ultra-wideband base stations are pre-arranged in an indoor scene where the tag is located, a spatial three-dimensional rectangular coordinate system is established, and the TDOA original data of the tag is obtained by positioning the tag through the ultra-wideband base stations in a TDOA mode;
estimating and obtaining the estimated three-dimensional position of the label through a three-dimensional Chan algorithm by using the TDOA original data;
carrying out residual value weighting processing on the estimated three-dimensional position of the label;
and taking the pre-estimated three-dimensional position of the processed label as an initial value of a Gaussian-Newton iterative algorithm, and obtaining the three-dimensional space position of the label through iterative operation.
With reference to the first aspect, in a first implementable manner of the first aspect, the establishing a spatial three-dimensional rectangular coordinate system includes:
setting label coordinates (X, y, z) and base station coordinates (X)i，Yi，Zi) C is the propagation speed of electromagnetic wave in air, tiFor the time of the ultra-wideband signal from the tag to the ith base station, ti，1For the time difference, t, from the tag to the ith base station and to the 1 st base station for the ultra-wideband signali，1＝ti-t1，riIs the true spatial distance, r, of the tag to the ith base stationi＝ctiAnd is represented by
Figure BDA0002291723190000021
ri,1Is the distance difference r between the tag to the ith base station and the 1 st base stationi,1＝ri-r1Is shown as
Figure BDA0002291723190000022
With reference to the first aspect, in a second implementation manner of the first aspect, the obtaining, by using the TDOA raw data and through a three-dimensional space Chan algorithm, an estimated three-dimensional position of the tag includes:
according to
Figure BDA0002291723190000023
Is pushed to
Figure BDA0002291723190000024
Eliminating high-order terms:
Figure BDA0002291723190000025
wherein,
Figure BDA0002291723190000026
shift term by zaEstablishing a linear equation as the unknowns yields:
h＝Gaza；
in the formula:
Figure BDA0002291723190000027
is provided with
Figure BDA0002291723190000028
Is zaThe true value, i.e. the true value of the label, the signal delay random error n generated by TDOA, the mean value is zero, and the covariance matrix is
Figure BDA0002291723190000029
TDOA distance measurement
Figure BDA00022917231900000210
Is riAnd (3) the true value, namely the error vector equation and the analytic expression are as follows:
Figure BDA00022917231900000211
Figure BDA00022917231900000212
therein, it is known
Figure BDA00022917231900000213
Determining an error covariance matrix psi ═ E (psiT)＝c2BQB, hypothesis zaAre independent of each other, to zaA preliminary weighted least squares estimation is made,
Figure BDA00022917231900000214
in practical cases, zaMiddle element r1Associated with (x, y, z), let z be sufficiently small, assuming that there is enough noise e presentaBecomes a random vector whose mean value is the true value, at which time GaH and zaIs shown as
Figure BDA00022917231900000215
Bringing the above formula into
Figure BDA00022917231900000216
Can obtain the following in turn:
Figure BDA00022917231900000217
Figure BDA00022917231900000218
Figure BDA00022917231900000219
let eiI is 1,2,3,4 is noise, z isaThe elements are represented as
za＝[x0+e1,y0+e2,z0+e3,r1 0+e4]T；
A new linear equation can be established as
ψ'＝h'-Ga'za'；
Wherein:
Figure BDA0002291723190000031
to za' weighted least squares estimation
Figure BDA0002291723190000032
Where psi' error covariance matrix is at eiWhen the smaller, the higher-order terms in psi' calculation are ignored, and the approximation processing is carried out to obtain:
Ψ'＝E[ψ'ψ'T]＝4B'cov(za)B'；
B'＝diag{x0-X1,y0-Y1,z0-Z1,r1 0}；
cov(za) Contains unknown true value
Figure BDA0002291723190000033
By GaApproximate substitution, for true coordinates unknown to the tag contained in B
Figure BDA0002291723190000034
Estimated result
Figure BDA0002291723190000035
Approximate calculation is carried out, and the label coordinate z can be finally estimated according to the prior informationtIs composed of
Figure BDA0002291723190000036
With reference to the first aspect, in a third implementation manner of the first aspect, the performing residual value weighting processing on the estimated three-dimensional position of the tag includes:
selecting one base station as a positioning reference station, and grouping the rest M-1 base stations, wherein the participation amount of the base stations is at least 4, and the grouping is performed
Figure BDA0002291723190000037
Seed combination, the kth combination is denoted as SkThe set of base stations corresponding to the combination is shown, and the coordinates of the label estimated by the Chan algorithm under the combination are recorded as zt,k＝[xk,yk,zk]TN, the coordinate of the ith base station is denoted as ai＝[Xi,Yi,Zi]；
Establishing a three-dimensional square residual value function:
Figure BDA0002291723190000038
in the formula
Figure BDA0002291723190000041
Assuming that each group of positions is estimated by the Chan algorithm, the weight is wk，wk＝1/p(zt,k,Sk) Then the tag location is estimated as
Figure BDA0002291723190000042
With reference to the first aspect, in a fourth implementation manner of the first aspect, obtaining the three-dimensional spatial position of the tag through iterative operation by using the estimated three-dimensional position of the processed tag as an initial value of a gaussian-newton iterative algorithm includes:
setting an applicable coordinate parameter, and establishing a Gauss-Newton iteration function prototype corresponding to three-dimensional positioning;
the real coordinate of the label is (X, y, z), the ith base station coordinate is (X)i,Yi,Zi) And is provided with mi,kThe kth TDOA measurement, i.e., m, measured for the ith base stationi,k＝ri,k-r1,kEstablishing a function prototype of
fk(x,y,z,Xi,Yi,Zi)＝uk＝mi,k-ek,k＝1,2,...,n，
In the formula, ekIs the statistically distributed random variable error with mean value of zero and error covariance matrix Re＝[cij]I row and j column element c in matrixij＝(eij)；
Using residue passing through squareValue weighted tag coordinate estimate zrw(xv,yv,zv) Establishing a relation with the real coordinates (x, y, z) of the label, and performing function prototype fkPerforming Taylor series expansion, and reserving an argument to obtain:
x＝xv+δx,y＝yv+δy,z＝zv+δz，
fkv+ak1δx+ak2δy+ak3δz＝mi,k-ek，
in the formula:
Figure BDA0002291723190000043
Figure BDA0002291723190000044
defining a matrix and a vector:
Figure BDA0002291723190000045
Figure BDA0002291723190000051
establishing a new equation of L ═ A delta + e (23);
by performing a weighted least squares estimation, a solution for δ can be obtained
δ＝[ATRe -1A]-1ATRe -1L；
Construction of weighted error covariance matrix W using residual squaredeIs composed of
Figure BDA0002291723190000052
Weighted error covariance matrix W to be constructedeBy-pass, a least squares solution of δ can be obtained
δ＝[ATWe -1A]-1ATWe -1L；
Z after iterationrw'(xv',yv',zv') is
xv'＝xv+δx,yv'＝yv+δy,zv'＝zv+δz；
Repeating the iteration when the | delta is satisfiedx|+|δy|+|δzIf the | < epsilon and the epsilon is sufficiently small, Gaussian-Newton iteration converges, and finally the optimal estimated three-dimensional space position of the label can be obtained.
In a second aspect, the present invention provides an indoor three-dimensional positioning device based on ultra-wideband communication, the device comprising:
the device comprises an acquisition unit, a detection unit and a processing unit, wherein the acquisition unit is used for acquiring TDOA (time difference of arrival) original data of a label, more than 4 ultra-wideband base stations are arranged in an indoor scene where the label is located in advance, a spatial three-dimensional rectangular coordinate system is established, and the TDOA original data of the label is obtained by positioning the label by the ultra-wideband base stations in a TDOA mode;
the estimating unit is used for estimating and obtaining the estimated three-dimensional position of the label through a three-dimensional space Chan algorithm by utilizing the TDOA original data;
the processing unit is used for carrying out residual value weighting processing on the estimated three-dimensional position of the label;
and the computing unit is used for taking the estimated three-dimensional position of the processed label as an initial value of a Gaussian-Newton iterative algorithm, and obtaining the three-dimensional space position of the label through iterative operation.
With reference to the second aspect, in a first implementable manner of the second aspect, the establishing a spatial three-dimensional rectangular coordinate system includes:
setting label coordinates (X, y, z) and base station coordinates (X)i，Yi，Zi) C is the propagation speed of electromagnetic wave in air, tiFor the time of the ultra-wideband signal from the tag to the ith base station, ti，1For the time difference, t, from the tag to the ith base station and to the 1 st base station for the ultra-wideband signali，1＝ti-t1，riIs the true spatial distance, r, of the tag to the ith base stationi＝ctiAnd is represented by
Figure BDA0002291723190000053
ri,1Is the distance difference r between the tag to the ith base station and the 1 st base stationi,1＝ri-r1Is shown as
Figure BDA0002291723190000061
With reference to the second aspect, in a second implementable manner of the second aspect, the estimating unit is configured to:
according to
Figure BDA0002291723190000062
Is pushed to
Figure BDA0002291723190000063
Eliminating high-order terms:
Figure BDA0002291723190000064
wherein,
Figure BDA0002291723190000065
shift term by zaEstablishing a linear equation as the unknowns yields:
h＝Gaza；
in the formula:
Figure BDA0002291723190000066
is provided with
Figure BDA0002291723190000067
Is zaThe true value, i.e. the true value of the label, the signal delay random error n generated by TDOA, the mean value is zero, and the covariance matrix is
Figure BDA0002291723190000068
TDOA distance measurement
Figure BDA0002291723190000069
Is riAnd (3) the true value, namely the error vector equation and the analytic expression are as follows:
Figure BDA00022917231900000610
Figure BDA00022917231900000611
therein, it is known
Figure BDA00022917231900000612
Determining an error covariance matrix psi ═ E (psiT)＝c2BQB, hypothesis zaAre independent of each other, to zaA preliminary weighted least squares estimation is made,
Figure BDA00022917231900000613
in practical cases, zaMiddle element r1Associated with (x, y, z), let z be sufficiently small, assuming that there is enough noise e presentaBecomes a random vector whose mean value is the true value, at which time GaH and zaIs shown as
Figure BDA00022917231900000614
Bringing the above formula into
Figure BDA00022917231900000615
Can obtain the following in turn:
Figure BDA00022917231900000616
Figure BDA00022917231900000617
Figure BDA00022917231900000618
let eiI is 1,2,3,4 is noise, z isaThe elements are represented as
za＝[x0+e1,y0+e2,z0+e3,r1 0+e4]T；
A new linear equation can be established as
ψ'＝h'-Ga'za'；
Wherein:
Figure BDA0002291723190000071
to za' weighted least squares estimation
Figure BDA0002291723190000072
Where psi' error covariance matrix is at eiWhen the smaller, the higher-order terms in psi' calculation are ignored, and the approximation processing is carried out to obtain:
Ψ'＝E[ψ'ψ'T]＝4B'cov(za)B'；
B'＝diag{x0-X1,y0-Y1,z0-Z1,r1 0}；
cov(za) Contains unknown true value
Figure BDA0002291723190000073
By GaApproximate substitution, for true coordinates unknown to the tag contained in B
Figure BDA0002291723190000074
Estimated result
Figure BDA0002291723190000075
Approximate calculation is carried out, and the label coordinate z can be finally estimated according to the prior informationtIs composed of
Figure BDA0002291723190000076
With reference to the second aspect, in a third implementable manner of the second aspect, the processing unit is configured to:
selecting one base station as a positioning reference station, and grouping the rest M-1 base stations, wherein the participation amount of the base stations is at least 4, and the grouping is performed
Figure BDA0002291723190000077
Seed combination, the kth combination is denoted as SkThe set of base stations corresponding to the combination is shown, and the coordinates of the label estimated by the Chan algorithm under the combination are recorded as zt,k＝[xk,yk,zk]TN, the coordinate of the ith base station is denoted as ai＝[Xi,Yi,Zi]；
Establishing a three-dimensional square residual value function:
Figure BDA0002291723190000078
in the formula,
Figure BDA0002291723190000079
assuming that each group of positions is estimated by the Chan algorithm, the weight is wk，wk＝1/p(zt,k,Sk) Then the tag location is estimated as
Figure BDA0002291723190000081
With reference to the second aspect, in a fourth implementable manner of the second aspect, the computing unit is configured to:
setting an applicable coordinate parameter, and establishing a Gauss-Newton iteration function prototype corresponding to three-dimensional positioning;
the real coordinate of the label is (X, y, z), the ith base station coordinate is (X)i,Yi,Zi) And is provided with mi,kThe kth TDOA measurement, i.e., m, measured for the ith base stationi,k＝ri,k-r1,kEstablishing a function prototype of
fk(x,y,z,Xi,Yi,Zi)＝uk＝mi,k-ek,k＝1,2,...,n，
In the formula, ekIs the statistically distributed random variable error with mean value of zero and error covariance matrix Re＝[cij]I row and j column element c in matrixij＝(eij)；
Tag coordinate estimation value z weighted by square residual valuerw(xv,yv,zv) Establishing a relation with the real coordinates (x, y, z) of the label, and performing function prototype fkPerforming Taylor series expansion, and reserving an argument to obtain:
x＝xv+δx,y＝yv+δy,z＝zv+δz，
fkv+ak1δx+ak2δy+ak3δz＝mi,k-ek，
in the formula:
Figure BDA0002291723190000082
Figure BDA0002291723190000083
defining a matrix and a vector:
Figure BDA0002291723190000084
Figure BDA0002291723190000085
establishing a new equation of L ═ A delta + e;
by performing a weighted least squares estimation, a solution for δ can be obtained
δ＝[ATRe -1A]-1ATRe -1L；
Construction of weighted error covariance matrix W using residual squaredeIs composed of
Figure BDA0002291723190000091
Weighted error covariance matrix W to be constructedeBy-pass, a least squares solution of δ can be obtained
δ＝[ATWe -1A]-1ATWe -1L；
Z after iterationrw'(xv',yv',zv') is
xv'＝xv+δx,yv'＝yv+δy,zv'＝zv+δz；
Repeating the iteration when the | delta is satisfiedx|+|δy|+|δzIf the | < epsilon and the epsilon is sufficiently small, Gaussian-Newton iteration converges, and finally the optimal estimated three-dimensional space position of the label can be obtained.
The invention has the following beneficial effects:
the invention provides an indoor three-dimensional positioning method and device based on ultra-wideband communication, more than 4 ultra-wideband base stations are arranged aiming at an indoor scene, a tag is positioned in a TDOA mode, two times of weighted least square estimation are carried out on TDOA original data obtained by an ultra-wideband system by utilizing a Chan algorithm to obtain an estimated value of a three-dimensional position of the tag, a residual value weighting method is utilized to carry out smoothing processing on a non-line-of-sight error generated by the ultra-wideband system to obtain a further accurate estimated value of the three-dimensional position of the tag, the obtained estimated value is used as an initial value of Gaussian-Newton iteration, and the three-dimensional position of the tag is finally obtained through repeated iteration operation.
Drawings
In order to more clearly illustrate the technical solution of the present invention, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious to those skilled in the art that other drawings can be obtained according to the drawings without any inventive exercise.
Fig. 1 is a flowchart of an indoor three-dimensional positioning method based on ultra-wideband communication according to an embodiment of the present invention.
Fig. 2 is a schematic diagram of a TDOA mode three-dimensional space positioning in an indoor three-dimensional positioning method based on ultra-wideband communication according to an embodiment of the present invention.
Fig. 3 is a schematic diagram of an indoor three-dimensional rectangular coordinate system in an indoor three-dimensional positioning method based on ultra-wideband communication according to an embodiment of the present invention.
Fig. 4 is a schematic diagram of an indoor three-dimensional positioning device based on ultra-wideband communication according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below with reference to the specific embodiments of the present invention and the accompanying drawings. It is to be understood that the described embodiments are merely exemplary of the invention, and not restrictive of the full scope of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention. The technical solutions provided by the embodiments of the present invention are described in detail below with reference to the accompanying drawings.
Referring to fig. 1, in an embodiment of the present invention, an execution main body of the method may be a processor, and the method specifically includes the following steps:
step S101, TDOA original data of a tag is obtained, wherein more than 4 ultra-wideband base stations are arranged in an indoor scene where the tag is located in advance, a spatial three-dimensional rectangular coordinate system is established, and the TDOA original data of the tag is obtained by positioning the tag through the ultra-wideband base stations in a TDOA mode.
As shown in fig. 2 and fig. 3, the ultra-wideband base stations are distributed in the indoor space according to the TDOA positioning principle, and because of the three-dimensional positioning, the participation amount of the base stations is at least 4, including at least base station 1, base station 2, base station 3 and base station 4.
In this embodiment, a spatial three-dimensional rectangular coordinate system is established, and the base station coordinates (X, y, z) and the label coordinates (X, y, z) are seti，Yi，Zi) And c is the propagation speed of the electromagnetic wave in the air. t is tiFor the time of the ultra-wideband signal from the tag to the ith base station, ti，1For the time difference, t, from the tag to the ith base station and to the 1 st base station for the ultra-wideband signali，1＝ti-t1。riIs the true spatial distance, r, of the tag to the ith base stationi＝ctiAnd is represented as:
Figure BDA0002291723190000101
ri,1is the distance difference r between the tag to the ith base station and the 1 st base stationi，1＝ri-r1Expressed as:
Figure BDA0002291723190000102
and S102, estimating to obtain an estimated three-dimensional position of the label through a three-dimensional space Chan algorithm by using TDOA original data.
In this embodiment, step S102 may specifically include the following steps:
step 2.1: deducing a suitable three-dimensional Chan algorithm, obtaining preliminary estimation of the three-dimensional space position of the label through the Chan algorithm, and obtaining the label three-dimensional space position according to the preliminary estimation
Figure BDA0002291723190000103
Can be pushed to
Figure BDA0002291723190000104
Eliminating high-order terms can be obtained:
Figure BDA0002291723190000105
wherein,
Figure BDA0002291723190000106
shift term by zaEstablishing a linear equation as the unknowns yields:
h＝Gazaformula (4)
In the formula:
Figure BDA0002291723190000111
step 2.2: because of the signal delay error of the ultra-wideband system in practical situation, it is designed
Figure BDA0002291723190000112
Is zaThe true value, i.e. the true value of the label, the signal delay random error n generated by TDOA, the mean value is zero, and the covariance matrix is
Figure BDA0002291723190000113
TDOA distance measurement
Figure BDA0002291723190000114
Is riAnd the true value, the error vector equation and the analytic expression are respectively as follows:
Figure BDA0002291723190000115
Figure BDA0002291723190000116
therein, it is known
Figure BDA0002291723190000117
Obtained by the formula (6)Error covariance matrix psi ═ E (psi ^ PSI)T)＝c2BQB, hypothesis zaAre independent of each other, to zaPerforming preliminary weighted least square estimation to obtain:
Figure BDA0002291723190000118
step 2.3: in practical cases, zaMiddle element r1Associated with (x, y, z), let z be sufficiently small, assuming that there is enough noise e presentaBecomes a random vector whose mean value is the true value, at which time GaH and zaExpressed as:
Figure BDA0002291723190000119
by bringing formula (8) into formula (5), the following can be obtained
Figure BDA00022917231900001110
Figure BDA00022917231900001111
Figure BDA00022917231900001112
Let eiI is 1,2,3,4 is noise, z isaThe elements are represented as:
za＝[x0+e1,y0+e2,z0+e3,r1 0+e4]Tformula (12)
The new linear equation can be established as:
ψ'＝h'-Ga'za' type (13)
Wherein:
Figure BDA0002291723190000121
to za' minimization of weightingThe second-product estimation yields:
Figure BDA0002291723190000122
where psi' error covariance matrix is at eiWhen smaller, the high-order term obtained by calculating psi' is ignored, and the approximation is carried out
Ψ'＝E[ψ'ψ'T]＝4B'cov(za) B' type (15)
B'＝diag{x0-X1,y0-Y1,z0-Z1,r1 0} formula (16)
cov(za) Contains unknown true value
Figure BDA0002291723190000123
By GaApproximate substitution, and the actual coordinates of the unknown tag contained in B' are estimated using equation (7)
Figure BDA0002291723190000124
Approximate calculation is carried out, and the label coordinate z can be finally estimated according to the prior informationtComprises the following steps:
Figure BDA0002291723190000125
and step S103, carrying out residual value weighting processing on the estimated three-dimensional position of the label.
In this embodiment, step S103 may specifically include:
step 3.1: selecting one base station as a positioning reference station, grouping the rest M-1 base stations, wherein the participation amount of the base stations is at least 4 due to three-dimensional positioning, and the grouping is total
Figure BDA0002291723190000126
Seed combination, the kth combination is denoted as SkThe set of base stations corresponding to the combination is shown, and the coordinates of the label estimated by the Chan algorithm under the combination are recorded as zt,k＝[xk,yk,zk]T,k＝1,2, the coordinates of the ith base station are recorded as Ai＝[Xi,Yi,Zi]。
Step 3.2: establishing a three-dimensional square residual value function:
Figure BDA0002291723190000127
in the formula,
Figure BDA0002291723190000128
assuming that each group of positions is estimated by the Chan algorithm, the weight is wk，wk＝1/p(zt,k,Sk) Then the tag location estimate is:
Figure BDA0002291723190000129
and step S104, taking the estimated three-dimensional position of the processed label as an initial value of a Gaussian-Newton iterative algorithm, and obtaining the three-dimensional space position of the label through iterative operation.
In this embodiment, step S104 may specifically include:
step 4.1: setting suitable coordinate parameters, and establishing a Gauss-Newton iteration function prototype corresponding to three-dimensional positioning.
The real coordinate of the label is (X, y, z), the ith base station coordinate is (X)i,Yi,Zi) And is provided with mi,kThe kth TDOA measurement, i.e., m, measured for the ith base stationi,k＝ri,k-r1,kEstablishing a function prototype as follows:
fk(x,y,z,Xi,Yi,Zi)＝uk＝mi,k- e k1,2, 1, n formula (20)
In the formula, ekIs the statistically distributed random variable error with mean value of zero and error covariance matrix Re＝[cij]I row and j column element c in matrixij＝(eij)。
Step 4.2: using residue passing through squareValue weighted tag coordinate estimate zrw(xv,yv,zv) Establishing a relation with the real coordinates (x, y, z) of the label, and performing function prototype fkPerforming Taylor series expansion, and reserving an argument to obtain
x＝xv+δx,y＝yv+δy,z＝zv+δzFormula (21)
fkv+ak1δx+ak2δy+ak3δz＝mi,k-ekFormula (22)
In the formula:
Figure BDA0002291723190000131
Figure BDA0002291723190000132
defining matrices and vectors
Figure BDA0002291723190000133
Figure BDA0002291723190000134
Equation (22) may establish a new equation as:
l as a δ + e (23)
The solution to δ can be obtained by doing a weighted least squares estimation:
δ＝[ATRe -1A]-1ATRe -1l type (24)
Step 4.3: due to error ekThe variances being equal independently of one another, i.e. Reσ i, but in practice, ReSince the prior information of TDOA measurements is unknown and cannot be estimated, ReThe value can effectively inhibit the non-line-of-sight error interference, and the weighted error covariance matrix W constructed by using the residual value square is providedeIs composed of
Figure BDA0002291723190000141
Weighted error covariance matrix W to be constructedeIn equation (24), the least squares solution of δ can be found as:
δ＝[ATWe -1A]-1ATWe -1l type (25)
Z after iterationrw'(xv',yv',zv') is:
xv'＝xv+δx,yv'＝yv+δy,zv'＝zv+δzformula (26)
Repeating the iteration when the | delta is satisfiedx|+|δy|+|δzIf the | < epsilon and the epsilon is sufficiently small, Gaussian-Newton iteration converges, and finally the optimal estimated three-dimensional space position of the label can be obtained.
Referring to fig. 4, the present invention further provides an indoor three-dimensional positioning device based on ultra-wideband communication, the device including:
the obtaining unit 401 is configured to obtain TDOA raw data of a tag, where more than 4 ultra-wideband base stations are pre-arranged in an indoor scene where the tag is located, and a spatial three-dimensional rectangular coordinate system is established, where the TDOA raw data of the tag is obtained by locating the tag by the ultra-wideband base stations in a TDOA manner.
The estimating unit 402 is configured to estimate an estimated three-dimensional position of the tag through a three-dimensional Chan algorithm by using TDOA raw data.
The processing unit 403 is configured to perform residual value weighting processing on the estimated three-dimensional position of the tag.
And the calculating unit 404 is configured to use the estimated three-dimensional position of the processed tag as an initial value of a gaussian-newton iterative algorithm, and obtain a three-dimensional spatial position of the tag through iterative operation.
In this embodiment, establishing a spatial three-dimensional rectangular coordinate system includes: setting label coordinates (X, y, z) and base station coordinates (X)i，Yi，Zi) C is the propagation speed of electromagnetic wave in air, tiFor the time of the ultra-wideband signal from the tag to the ith base station, ti，1For the time difference, t, from the tag to the ith base station and to the 1 st base station for the ultra-wideband signali，1＝ti-t1，riIs the true spatial distance, r, of the tag to the ith base stationi＝ctiAnd is represented by
Figure BDA0002291723190000142
ri,1Is the distance difference r between the tag to the ith base station and the 1 st base stationi,1＝ri-r1Is shown as
Figure BDA0002291723190000143
In this embodiment, the estimating unit 402 is configured to: according to
Figure BDA0002291723190000144
Is pushed to
Figure BDA0002291723190000145
Eliminating high-order terms:
Figure BDA0002291723190000146
wherein,
Figure BDA0002291723190000147
shift term by zaEstablishing a linear equation as the unknowns yields:
h＝Gaza；
in the formula:
Figure BDA0002291723190000151
Figure BDA0002291723190000152
is provided with
Figure BDA0002291723190000153
Is zaThe true value, i.e. the true value of the label, the signal delay random error n generated by TDOA, the mean value is zero, and the covariance matrix is
Figure BDA0002291723190000154
TDOA distance measurement
Figure BDA0002291723190000155
Is riAnd (3) the true value, namely the error vector equation and the analytic expression are as follows:
Figure BDA0002291723190000156
Figure BDA0002291723190000157
therein, it is known
Figure BDA0002291723190000158
Determining an error covariance matrix psi ═ E (psiT)＝c2BQB, hypothesis zaAre independent of each other, to zaA preliminary weighted least squares estimation is made,
Figure BDA0002291723190000159
in practical cases, zaMiddle element r1Associated with (x, y, z), let z be sufficiently small, assuming that there is enough noise e presentaBecomes a random vector whose mean value is the true value, at which time GaH and zaIs shown as
Figure BDA00022917231900001510
Bringing the above formula into
Figure BDA00022917231900001511
Can obtain the following in turn:
Figure BDA00022917231900001512
Figure BDA00022917231900001513
Figure BDA00022917231900001514
let eiI is 1,2,3,4 is noise, z isaThe elements are represented as
za＝[x0+e1,y0+e2,z0+e3,r1 0+e4]T；
A new linear equation can be established as
ψ'＝h'-Ga'za'；
Wherein:
Figure BDA0002291723190000161
Figure BDA0002291723190000162
Figure BDA0002291723190000163
to za' weighted least squares estimation
Figure BDA0002291723190000164
Where psi' error covariance matrix is at eiWhen the smaller, the higher-order terms in psi' calculation are ignored, and the approximation processing is carried out to obtain:
Ψ'＝E[ψ'ψ'T]＝4B'cov(za)B'；
B'＝diag{x0-X1,y0-Y1,z0-Z1,r1 0}；
cov(za) Contains unknown true value
Figure BDA0002291723190000165
By GaApproximate substitution, for true coordinates unknown to the tag contained in B
Figure BDA0002291723190000166
Estimated result
Figure BDA0002291723190000167
Approximate calculation is carried out, and the label coordinate z can be finally estimated according to the prior informationtIs composed of
Figure BDA0002291723190000168
In this embodiment, the processing unit 403 is configured to: selecting one base station as a positioning reference station, and grouping the rest M-1 base stations, wherein the participation amount of the base stations is at least 4, and the grouping is performed
Figure BDA0002291723190000169
Seed combination, the kth combination is denoted as SkThe set of base stations corresponding to the combination is shown, and the coordinates of the label estimated by the Chan algorithm under the combination are recorded as zt,k＝[xk,yk,zk]TN, the coordinate of the ith base station is denoted as ai＝[Xi,Yi,Zi]；
Establishing a three-dimensional square residual value function:
Figure BDA00022917231900001610
in the formula
Figure BDA0002291723190000171
Figure BDA0002291723190000172
Assuming that each group of positions is estimated by the Chan algorithm, the weight is wk，wk＝1/p(zt,k,Sk) Then the tag location is estimated as
Figure BDA0002291723190000173
In this embodiment, the calculating unit 404 is configured to: setting an applicable coordinate parameter, and establishing a Gauss-Newton iteration function prototype corresponding to three-dimensional positioning;
the real coordinate of the label is (X, y, z), the ith base station coordinate is (X)i,Yi,Zi) And is provided with mi,kThe kth TDOA measurement, i.e., m, measured for the ith base stationi,k＝ri,k-r1,kEstablishing a function prototype as fk(x,y,z,Xi,Yi,Zi)＝uk＝mi,k-e k1,2,.., n, wherein e iskIs the statistically distributed random variable error with mean value of zero and error covariance matrix Re＝[cij]I row and j column element c in matrixij＝(eij)；
Tag coordinate estimation value z weighted by square residual valuerw(xv,yv,zv) Establishing a relation with the real coordinates (x, y, z) of the label, and performing function prototype fkPerforming Taylor series expansion, and reserving an argument to obtain:
x＝xv+δx,y＝yv+δy,z＝zv+δz，
fkv+ak1δx+ak2δy+ak3δz＝mi,k-ek，
in the formula:
Figure BDA0002291723190000174
Figure BDA0002291723190000175
Figure BDA0002291723190000176
Figure BDA0002291723190000177
defining a matrix and a vector:
Figure BDA0002291723190000178
Figure BDA0002291723190000181
establishing a new equation of L ═ A delta + e (23);
by performing a weighted least squares estimation, a solution for δ can be obtained
δ＝[ATRe -1A]-1ATRe -1L；
Construction of weighted error covariance matrix W using residual squaredeIs composed of
Figure BDA0002291723190000182
Weighted error covariance matrix W to be constructedeBy-pass, a least squares solution of δ can be obtained
δ＝[ATWe -1A]-1ATWe -1L；
Z after iterationrw'(xv',yv',zv') is
xv'＝xv+δx,yv'＝yv+δy,zv'＝zv+δz；
The iteration is repeated, and the result is obtained,when | δ is satisfiedx|+|δy|+|δzIf the | < epsilon and the epsilon is sufficiently small, Gaussian-Newton iteration converges, and finally the optimal estimated three-dimensional space position of the label can be obtained.
The embodiment of the invention also provides a storage medium, and the storage medium stores a computer program, and the computer program is executed by a processor to realize part or all of the steps in each embodiment of the indoor three-dimensional positioning method based on the ultra-wideband communication. The storage medium may be a magnetic disk, an optical disk, a Read-only memory (ROM) or a Random Access Memory (RAM).
Those skilled in the art will readily appreciate that the techniques of the embodiments of the present invention may be implemented as software plus a required general purpose hardware platform. Based on such understanding, the technical solutions in the embodiments of the present invention may be essentially or partially implemented in the form of a software product, which may be stored in a storage medium, such as ROM/RAM, magnetic disk, optical disk, etc., and includes several instructions for enabling a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method according to the embodiments or some parts of the embodiments.
The same and similar parts in the various embodiments in this specification may be referred to each other. In particular, for the indoor three-dimensional positioning device embodiment based on ultra-wideband communication, since it is basically similar to the method embodiment, the description is relatively simple, and for the relevant points, refer to the description in the method embodiment.
The above-described embodiments of the present invention should not be construed as limiting the scope of the present invention.

Claims (10)

1. An indoor three-dimensional positioning method based on ultra-wideband communication is characterized by comprising the following steps:
acquiring TDOA original data of a tag, wherein more than 4 ultra-wideband base stations are pre-arranged in an indoor scene where the tag is located, a spatial three-dimensional rectangular coordinate system is established, and the TDOA original data of the tag is obtained by positioning the tag through the ultra-wideband base stations in a TDOA mode;
estimating and obtaining the estimated three-dimensional position of the label through a three-dimensional Chan algorithm by using the TDOA original data;
carrying out residual value weighting processing on the estimated three-dimensional position of the label;
and taking the pre-estimated three-dimensional position of the processed label as an initial value of a Gaussian-Newton iterative algorithm, and obtaining the three-dimensional space position of the label through iterative operation.
2. The method of claim 1, wherein establishing a spatial three-dimensional rectangular coordinate system comprises:
setting label coordinates (X, y, z) and base station coordinates (X)i，Yi，Zi) C is the propagation speed of electromagnetic wave in air, tiFor the time of the ultra-wideband signal from the tag to the ith base station, ti，1For the time difference, t, from the tag to the ith base station and to the 1 st base station for the ultra-wideband signali，1＝ti-t1，riIs the true spatial distance, r, of the tag to the ith base stationi＝ctiAnd is represented by
Figure FDA0002291723180000011
ri,1Is the distance difference r between the tag to the ith base station and the 1 st base stationi,1＝ri-r1Is shown as
Figure FDA0002291723180000012
3. The method of claim 1, wherein estimating the estimated three-dimensional position of the tag using the TDOA raw data by a three-dimensional Chan algorithm comprises:
according to
Figure FDA0002291723180000013
Is pushed to
Figure FDA0002291723180000014
Eliminating high-order terms:
Figure FDA0002291723180000015
wherein,
Figure FDA0002291723180000016
shift term by zaEstablishing a linear equation as the unknowns yields:
h＝Gaza；
in the formula:
Figure FDA0002291723180000017
Figure FDA0002291723180000018
is provided with
Figure FDA0002291723180000019
Is zaThe true value, i.e. the true value of the label, the signal delay random error n generated by TDOA, the mean value is zero, and the covariance matrix is
Figure FDA0002291723180000021
TDOA distance measurement
Figure FDA0002291723180000022
Is riAnd (3) the true value, namely the error vector equation and the analytic expression are as follows:
Figure FDA0002291723180000023
Figure FDA0002291723180000024
therein, it is known
Figure FDA0002291723180000025
Determining an error covariance matrix psi ═ E (psiT)＝c2BQB, hypothesis zaAre independent of each other, to zaA preliminary weighted least squares estimation is made,
Figure FDA0002291723180000026
in practical cases, zaMiddle element r1Associated with (x, y, z), let z be sufficiently small, assuming that there is enough noise e presentaBecomes a random vector whose mean value is the true value, at which time GaH and zaIs shown as
Figure FDA0002291723180000027
Bringing the above formula into
Figure FDA0002291723180000028
Can obtain the following in turn:
Figure FDA0002291723180000029
Figure FDA00022917231800000210
Figure FDA00022917231800000211
let eiI is 1,2,3,4 is noise, z isaThe elements are represented as
za＝[x0+e1,y0+e2,z0+e3,r1 0+e4]T；
A new linear equation can be established as
ψ'＝h'-Ga'za'；
Wherein:
Figure FDA00022917231800000212
Figure FDA00022917231800000213
Figure FDA00022917231800000214
to za' weighted least squares estimation
Figure FDA0002291723180000031
Where psi' error covariance matrix is at eiWhen the smaller, the higher-order terms in psi' calculation are ignored, and the approximation processing is carried out to obtain:
Ψ'＝E[ψ'ψ'T]＝4B'cov(za)B'；
B'＝diag{x0-X1,y0-Y1,z0-Z1,r1 0}；
cov(za) Contains unknown true value
Figure FDA0002291723180000032
By GaApproximate substitution, for true coordinates unknown to the tag contained in B
Figure FDA0002291723180000033
Estimated result
Figure FDA0002291723180000034
ApproximationCalculating, and finally estimating the coordinate z of the label according to the prior informationtIs composed of
Figure FDA0002291723180000035
4. The method of claim 1, wherein residual weighting the predicted three-dimensional position of the tag comprises:
selecting one base station as a positioning reference station, and grouping the rest M-1 base stations, wherein the participation amount of the base stations is at least 4, and the grouping is performed
Figure FDA0002291723180000036
Seed combination, the kth combination is denoted as SkThe set of base stations corresponding to the combination is shown, and the coordinates of the label estimated by the Chan algorithm under the combination are recorded as zt,k＝[xk,yk,zk]TN, the coordinate of the ith base station is denoted as ai＝[Xi,Yi,Zi]；
Establishing a three-dimensional square residual value function:
Figure FDA0002291723180000037
in the formula
Figure FDA0002291723180000038
Figure FDA0002291723180000039
Assuming that each group of positions is estimated by the Chan algorithm, the weight is wk，wk＝1/p(zt,k,Sk) Then the tag location is estimated as
Figure FDA00022917231800000310
5. The method of claim 1, wherein obtaining the three-dimensional spatial position of the tag through iterative operation by using the estimated three-dimensional position of the processed tag as an initial value of a gauss-newton iterative algorithm comprises:
setting an applicable coordinate parameter, and establishing a Gauss-Newton iteration function prototype corresponding to three-dimensional positioning;
the real coordinate of the label is (X, y, z), the ith base station coordinate is (X)i,Yi,Zi) And is provided with mi,kThe kth TDOA measurement, i.e., m, measured for the ith base stationi,k＝ri,k-r1,kEstablishing a function prototype as fk(x,y,z,Xi,Yi,Zi)＝uk＝mi,k-ek1,2,.., n, wherein e iskIs the statistically distributed random variable error with mean value of zero and error covariance matrix Re＝[cij]I row and j column element c in matrixij＝(eij)；
Tag coordinate estimation value z weighted by square residual valuerw(xv,yv,zv) Establishing a relation with the real coordinates (x, y, z) of the label, and performing function prototype fkPerforming Taylor series expansion, and reserving an argument to obtain:
x＝xv+δx,y＝yv+δy,z＝zv+δz，
fkv+ak1δx+ak2δy+ak3δz＝mi,k-ek，
in the formula:
Figure FDA0002291723180000041
Figure FDA0002291723180000042
Figure FDA0002291723180000043
Figure FDA0002291723180000044
defining a matrix and a vector:
Figure FDA0002291723180000045
Figure FDA0002291723180000046
establish a new equation of
L＝Aδ+e(23)；
By performing a weighted least squares estimation, a solution for δ can be obtained
δ＝[ATRe -1A]-1ATRe -1L；
Construction of weighted error covariance matrix W using residual squaredeIs composed of
Figure FDA0002291723180000051
Weighted error covariance matrix W to be constructedeBy-pass, a least squares solution of δ can be obtained
δ＝[ATWe -1A]-1ATWe -1L；
Z after iterationrw'(xv',yv',zv') is
xv'＝xv+δx,yv'＝yv+δy,zv'＝zv+δz；
Repeating the iteration when the | delta is satisfiedx|+|δy|+|δzIf the | < epsilon and the epsilon is sufficiently small, Gaussian-Newton iteration converges, and finally the optimal estimated three-dimensional space position of the label can be obtained.
6. An indoor three-dimensional positioning device based on ultra-wideband communication, characterized in that the device comprises:
the device comprises an acquisition unit, a detection unit and a processing unit, wherein the acquisition unit is used for acquiring TDOA (time difference of arrival) original data of a label, more than 4 ultra-wideband base stations are arranged in an indoor scene where the label is located in advance, a spatial three-dimensional rectangular coordinate system is established, and the TDOA original data of the label is obtained by positioning the label by the ultra-wideband base stations in a TDOA mode;
the estimating unit is used for estimating and obtaining the estimated three-dimensional position of the label through a three-dimensional space Chan algorithm by utilizing the TDOA original data;
the processing unit is used for carrying out residual value weighting processing on the estimated three-dimensional position of the label;
and the computing unit is used for taking the estimated three-dimensional position of the processed label as an initial value of a Gaussian-Newton iterative algorithm, and obtaining the three-dimensional space position of the label through iterative operation.
7. The apparatus of claim 6, wherein establishing a spatial three-dimensional rectangular coordinate system comprises:
setting label coordinates (X, y, z) and base station coordinates (X)i，Yi，Zi) C is the propagation speed of electromagnetic wave in air, tiFor the time of the ultra-wideband signal from the tag to the ith base station, ti，1For the time difference, t, from the tag to the ith base station and to the 1 st base station for the ultra-wideband signali，1＝ti-t1，riIs the true spatial distance, r, of the tag to the ith base stationi＝ctiAnd is represented by
Figure FDA0002291723180000052
ri,1Is the distance difference r between the tag to the ith base station and the 1 st base stationi,1＝ri-r1Is shown as
Figure FDA0002291723180000053
8. The apparatus of claim 6, wherein the estimation unit is to:
according to
Figure FDA0002291723180000054
Is pushed to
Figure FDA0002291723180000055
Eliminating high-order terms:
Figure FDA0002291723180000056
wherein,
Figure FDA0002291723180000057
shift term by zaEstablishing a linear equation as the unknowns yields:
h＝Gaza；
in the formula:
Figure FDA0002291723180000061
Figure FDA0002291723180000062
is provided with
Figure FDA0002291723180000063
Is zaThe true value, i.e. the true value of the label, the signal delay random error n generated by TDOA, the mean value is zero, and the covariance matrix is
Figure FDA0002291723180000064
TDOA distance measurement
Figure FDA0002291723180000065
Is riAnd (3) the true value, namely the error vector equation and the analytic expression are as follows:
Figure FDA0002291723180000066
Figure FDA0002291723180000067
therein, it is known
Figure FDA0002291723180000068
Determining an error covariance matrix psi ═ E (psiT)＝c2BQB, hypothesis zaAre independent of each other, to zaA preliminary weighted least squares estimation is made,
Figure FDA0002291723180000069
in practical cases, zaMiddle element r1Associated with (x, y, z), let z be sufficiently small, assuming that there is enough noise e presentaBecomes a random vector whose mean value is the true value, at which time GaH and zaIs shown as
Figure FDA00022917231800000610
Bringing the above formula into
Figure FDA00022917231800000611
Can obtain the following in turn:
Figure FDA00022917231800000612
Figure FDA00022917231800000613
Figure FDA00022917231800000614
let eiI is 1,2,3,4 is noise, z isaThe elements are represented as
za＝[x0+e1,y0+e2,z0+e3,r1 0+e4]T；
A new linear equation can be established as
ψ'＝h'-Ga'za'；
Wherein:
Figure FDA0002291723180000071
Figure FDA0002291723180000072
Figure FDA0002291723180000073
to za' weighted least squares estimation
Figure FDA0002291723180000074
Where psi' error covariance matrix is at eiWhen the smaller, the higher-order terms in psi' calculation are ignored, and the approximation processing is carried out to obtain:
Ψ'＝E[ψ'ψ'T]＝4B'cov(za)B'；
B'＝diag{x0-X1,y0-Y1,z0-Z1,r1 0}；
cov(za) Contains unknown true value
Figure FDA0002291723180000075
By GaApproximate substitution, for true coordinates unknown to the tag contained in B
Figure FDA0002291723180000076
Estimated result
Figure FDA0002291723180000077
Approximate calculation is carried out, and the label coordinate z can be finally estimated according to the prior informationtIs composed of
Figure FDA0002291723180000078
9. The apparatus as recited in claim 6, said processing unit to:
selecting one base station as a positioning reference station, and grouping the rest M-1 base stations, wherein the participation amount of the base stations is at least 4, and the grouping is performed
Figure FDA0002291723180000079
Seed combination, the kth combination is denoted as SkThe set of base stations corresponding to the combination is shown, and the coordinates of the label estimated by the Chan algorithm under the combination are recorded as zt,k＝[xk,yk,zk]TN, the coordinate of the ith base station is denoted as ai＝[Xi,Yi,Zi]；
Establishing a three-dimensional square residual value function:
Figure FDA0002291723180000081
in the formula
Figure FDA0002291723180000082
Figure FDA0002291723180000083
Assuming that each group of positions is estimated by the Chan algorithm, the weight is wk，wk＝1/p(zt,k,Sk) Then the tag location is estimated as
Figure FDA0002291723180000084
10. The apparatus of claim 6, wherein the computing unit is to:
setting an applicable coordinate parameter, and establishing a Gauss-Newton iteration function prototype corresponding to three-dimensional positioning;
the real coordinate of the label is (X, y, z), the ith base station coordinate is (X)i,Yi,Zi) And is provided with mi,kThe kth TDOA measurement, i.e., m, measured for the ith base stationi,k＝ri,k-r1,kEstablishing a function prototype as fk(x,y,z,Xi,Yi,Zi)＝uk＝mi,k-ek1,2,.., n, wherein e iskIs the statistically distributed random variable error with mean value of zero and error covariance matrix Re＝[cij]I row and j column element c in matrixij＝(eij)；
Tag coordinate estimation value z weighted by square residual valuerw(xv,yv,zv) Establishing a relation with the real coordinates (x, y, z) of the label, and performing function prototype fkPerforming Taylor series expansion, and reserving an argument to obtain:
x＝xv+δx,y＝yv+δy,z＝zv+δz，
fkv+ak1δx+ak2δy+ak3δz＝mi,k-ek，
in the formula:
Figure FDA0002291723180000085
Figure FDA0002291723180000086
Figure FDA0002291723180000087
Figure FDA0002291723180000088
defining a matrix and a vector:
Figure FDA0002291723180000091
Figure FDA0002291723180000092
establish a new equation of
L＝Aδ+e(23)；
By performing a weighted least squares estimation, a solution for δ can be obtained
δ＝[ATRe -1A]-1ATRe -1L；
Construction of weighted error covariance matrix W using residual squaredeIs composed of
Figure FDA0002291723180000093
Weighted error covariance matrix W to be constructedeBy-pass, a least squares solution of δ can be obtained
δ＝[ATWe -1A]-1ATWe -1L；
Z after iterationrw'(xv',yv',zv') is
xv'＝xv+δx,yv'＝yv+δy,zv'＝zv+δz；
Repeating the iteration when the | delta is satisfiedx|+|δy|+|δzIf the | < epsilon and the epsilon is sufficiently small, Gaussian-Newton iteration converges, and finally the optimal estimated three-dimensional space position of the label can be obtained.
CN201911182827.8A 2019-11-27 2019-11-27 Indoor three-dimensional positioning method and device based on ultra-wideband communication Pending CN110933630A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911182827.8A CN110933630A (en) 2019-11-27 2019-11-27 Indoor three-dimensional positioning method and device based on ultra-wideband communication

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911182827.8A CN110933630A (en) 2019-11-27 2019-11-27 Indoor three-dimensional positioning method and device based on ultra-wideband communication

Publications (1)

Publication Number Publication Date
CN110933630A true CN110933630A (en) 2020-03-27

Family

ID=69846707

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911182827.8A Pending CN110933630A (en) 2019-11-27 2019-11-27 Indoor three-dimensional positioning method and device based on ultra-wideband communication

Country Status (1)

Country Link
CN (1) CN110933630A (en)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111352444A (en) * 2020-04-23 2020-06-30 上海交通大学 Unmanned aerial vehicle outdoor mobile platform autonomous landing method and system based on wireless navigation
CN111559412A (en) * 2020-06-18 2020-08-21 中车株洲电力机车有限公司 Locomotive benchmarking method and locomotive benchmarking system based on UWB positioning
CN112738708A (en) * 2020-12-18 2021-04-30 中国电子科技集团公司第五十四研究所 Direction-finding and distance-measuring positioning method with partially unknown base station positions
CN112954591A (en) * 2021-02-10 2021-06-11 北京理工大学 Cooperative distributed positioning method and system
CN113063422A (en) * 2021-03-22 2021-07-02 中国科学院国家授时中心 Clock difference considered 5G terminal indoor positioning method
CN113671441A (en) * 2021-09-10 2021-11-19 哈尔滨工程大学 Indoor passive real-time positioning method based on ultra wide band technology
CN113804199A (en) * 2021-09-17 2021-12-17 中山大学 Combined positioning method and system based on Chan's algorithm and Newton's method
CN114089273A (en) * 2021-11-22 2022-02-25 电子科技大学 GPS and UWB based motion platform positioning method
CN114353795A (en) * 2021-12-06 2022-04-15 中南设计集团（武汉）工程技术研究院有限公司 Indoor three-dimensional positioning system and method based on UWB equipment
CN114598987A (en) * 2020-12-04 2022-06-07 大唐移动通信设备有限公司 Positioning method, positioning device, electronic equipment and computer readable storage medium
CN115022800A (en) * 2022-05-27 2022-09-06 国网江苏省电力有限公司电力科学研究院 Self-adaptive positioning method and system for indoor personnel of transformer substation

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080186231A1 (en) * 2007-02-05 2008-08-07 Daniel Aljadeff Dual bandwidth time difference of arrival (tdoa) system
CN109743701A (en) * 2018-12-04 2019-05-10 东南大学 Indoor 3-D positioning method based on ultra-wideband communications
CN110493742A (en) * 2019-08-28 2019-11-22 哈尔滨工程大学 A kind of indoor 3-D positioning method for ultra wide band

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080186231A1 (en) * 2007-02-05 2008-08-07 Daniel Aljadeff Dual bandwidth time difference of arrival (tdoa) system
CN109743701A (en) * 2018-12-04 2019-05-10 东南大学 Indoor 3-D positioning method based on ultra-wideband communications
CN110493742A (en) * 2019-08-28 2019-11-22 哈尔滨工程大学 A kind of indoor 3-D positioning method for ultra wide band

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111352444A (en) * 2020-04-23 2020-06-30 上海交通大学 Unmanned aerial vehicle outdoor mobile platform autonomous landing method and system based on wireless navigation
CN111559412A (en) * 2020-06-18 2020-08-21 中车株洲电力机车有限公司 Locomotive benchmarking method and locomotive benchmarking system based on UWB positioning
CN114598987A (en) * 2020-12-04 2022-06-07 大唐移动通信设备有限公司 Positioning method, positioning device, electronic equipment and computer readable storage medium
CN112738708A (en) * 2020-12-18 2021-04-30 中国电子科技集团公司第五十四研究所 Direction-finding and distance-measuring positioning method with partially unknown base station positions
CN112738708B (en) * 2020-12-18 2022-05-06 中国电子科技集团公司第五十四研究所 Direction-finding and distance-measuring positioning method with partially unknown base station positions
CN112954591B (en) * 2021-02-10 2022-04-15 北京理工大学 Cooperative distributed positioning method and system
CN112954591A (en) * 2021-02-10 2021-06-11 北京理工大学 Cooperative distributed positioning method and system
CN113063422A (en) * 2021-03-22 2021-07-02 中国科学院国家授时中心 Clock difference considered 5G terminal indoor positioning method
CN113671441A (en) * 2021-09-10 2021-11-19 哈尔滨工程大学 Indoor passive real-time positioning method based on ultra wide band technology
CN113671441B (en) * 2021-09-10 2023-10-03 哈尔滨工程大学 Indoor passive real-time positioning method based on ultra-wideband technology
CN113804199A (en) * 2021-09-17 2021-12-17 中山大学 Combined positioning method and system based on Chan's algorithm and Newton's method
CN113804199B (en) * 2021-09-17 2023-06-13 中山大学 Combined positioning method and system based on Chan's algorithm and Newton's method
CN114089273A (en) * 2021-11-22 2022-02-25 电子科技大学 GPS and UWB based motion platform positioning method
CN114089273B (en) * 2021-11-22 2023-05-26 电子科技大学 GPS and UWB-based motion platform positioning method
CN114353795A (en) * 2021-12-06 2022-04-15 中南设计集团（武汉）工程技术研究院有限公司 Indoor three-dimensional positioning system and method based on UWB equipment
CN115022800A (en) * 2022-05-27 2022-09-06 国网江苏省电力有限公司电力科学研究院 Self-adaptive positioning method and system for indoor personnel of transformer substation
CN115022800B (en) * 2022-05-27 2024-02-20 国网江苏省电力有限公司电力科学研究院 Self-adaptive positioning method and system for indoor personnel of transformer substation

Similar Documents

Publication Publication Date Title
CN110933630A (en) Indoor three-dimensional positioning method and device based on ultra-wideband communication
CN110493742B (en) Indoor three-dimensional positioning method for ultra-wideband
CN106405533B (en) Radar target combined synchronization and localization method based on constraint weighted least-squares
CN105898865B (en) Based on the co-located method of EKF and PF under the conditions of nonlinear and non-Gaussian
CN105954712B (en) The direct localization method of the multiple target of associated wireless electric signal complex envelope and carrier phase information
CN109100683A (en) Chan- weighted mass center indoor orientation method based on Kalman filtering
CN107371129A (en) The TDOA localization methods of indoor positioning based on height auxiliary amendment
CN104080165A (en) Indoor wireless sensor network positioning method based on TDOA
WO2016112758A1 (en) Method and apparatus for locating terminal
CN103379441A (en) Indoor positioning method based on region segmentation and curve fitting
CN115494450B (en) High-precision ultra-wideband indoor positioning tracking and control method and device
CN107633256A (en) Joint objective positioning and sensor registration method under a kind of multi-source ranging
CN110657806A (en) Position resolving method based on CKF, chan resolving and Savitzky-Golay smooth filtering
CN106792516B (en) 3-D positioning method based on radio communication base station
CN108318854B (en) Positioning method, positioning device, electronic equipment and readable storage medium
CN111263295B (en) WLAN indoor positioning method and device
CN107592654B (en) Method for positioning field intensity of same-frequency multiple radiation sources based on compressed sensing
CN108107421A (en) A kind of interior distance measuring method and device
CN108445446B (en) Passive speed measurement positioning method and device
CN111007456A (en) Robust non-line-of-sight deviation elimination positioning method capable of realizing time domain combination
CN110133586A (en) TOA combined synchronization and localization method based on linearity correction
CN111277950B (en) Positioning method, device and equipment based on arrival time difference and arrival frequency difference
CN113740802B (en) Signal source positioning method and system for performing matrix completion by using adaptive noise estimation
CN109115219A (en) A kind of indoor 3-D positioning method based on scene indices
CN113721191A (en) Signal source positioning method and system for improving matrix completion performance through self-adaptive rasterization

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
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20200327

WD01 Invention patent application deemed withdrawn after publication