CN110187306B - TDOA-PDR-MAP fusion positioning method applied to complex indoor space - Google Patents

TDOA-PDR-MAP fusion positioning method applied to complex indoor space Download PDF

Info

Publication number
CN110187306B
CN110187306B CN201910303807.5A CN201910303807A CN110187306B CN 110187306 B CN110187306 B CN 110187306B CN 201910303807 A CN201910303807 A CN 201910303807A CN 110187306 B CN110187306 B CN 110187306B
Authority
CN
China
Prior art keywords
positioning
target
tdoa
matrix
particle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910303807.5A
Other languages
Chinese (zh)
Other versions
CN110187306A (en
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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN201910303807.5A priority Critical patent/CN110187306B/en
Publication of CN110187306A publication Critical patent/CN110187306A/en
Application granted granted Critical
Publication of CN110187306B publication Critical patent/CN110187306B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • 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/0257—Hybrid positioning
    • 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/12—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 by co-ordinating position lines of different shape, e.g. hyperbolic, circular, elliptical or radial
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/029—Location-based management or tracking services
    • 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

Abstract

The invention discloses a TDOA-PDR-MAP fusion positioning method applied to a complex indoor space. According to the method, the TDOA position estimation based on the acoustic signal is used as the measurement information of the particle filter by utilizing the characteristic that the positioning technology based on the acoustic signal has high positioning precision in an unshielded indoor scene, so that the accumulated error of PDR positioning can be effectively made up, and the step length estimation precision in the PDR algorithm is improved. The PDR positioning technology is not easily influenced by environmental noise and multipath effect, positioning error in short distance is not large, and the PDR positioning model is used as a state equation of particle filtering, so that the defect that an acoustic signal is positioned in a non-line-of-sight environment can be overcome. Meanwhile, by utilizing the position constraint based on the indoor map information, on one hand, the positioning result can be effectively corrected, and on the other hand, certain specific actions can eliminate errors at one time, so that the accumulated errors of long-time positioning are reduced. Compared with the positioning precision of single TDOA positioning and single PDR positioning, the method has higher precision under the same environment.

Description

TDOA-PDR-MAP fusion positioning method applied to complex indoor space
Technical Field
The invention belongs to the field of indoor positioning, and particularly relates to a TDOA-PDR-MAP fusion positioning method applied to a complex indoor space.
Background
In recent years, with rapid development of network communication technologies represented by 4G, satellite positioning technologies represented by the big dipper, and smart mobile wearable devices represented by smartphones, location-based services have been more and more widely applied to daily life scenes such as trips, shopping, travels, and accommodations of humans. However, since location-based services are limited by technology and cost, some are concentrated in outdoor positioning scenes, and have low accuracy, which is in the range of 1m to 10m, and some are high in positioning accuracy but extremely high in cost, and are not suitable for consumer positioning scenes. In the indoor environment, location services and high-precision location services accurate to below 1m level exist in the related art, although the market of the demanding party is huge, various bottlenecks such as multipath effect, non-line-of-sight measurement, construction cost and transmission distance exist in the related art.
Among known higher-precision indoor positioning technologies, the bluetooth-based positioning technology has a short propagation distance, requiring a large number of base stations to be arranged, so that the positioning cost increases as the space expands. The Wi-Fi-based positioning technology algorithm is complex, and the real-time performance of Wi-Fi positioning is difficult to improve while the environmental noise interference is reduced. Ultra-wideband-based positioning technology, although extremely high in accuracy, is not applicable to consumer-level positioning scenarios due to its high cost. The indoor positioning technology based on the acoustic signals has the characteristics of low cost, easiness in operation, small operand, high precision and good compatibility of mobile equipment, and is very suitable for being applied to consumption-level indoor positioning scenes. However, in a complex indoor environment, due to the shielding of buildings and the influence of environmental noise, an acoustic signal is susceptible to the multipath effect of the signal and the influence of non-line-of-sight propagation, and since the propagation speed of the acoustic signal is low, when the moving speed of an object is high, time delay is also one of the problems based on the positioning of the acoustic signal. The positioning technology based on the PDR only needs to be based on general mobile equipment without additionally installing a base station, has low cost, is not easily influenced by environmental noise and multipath effect, has small positioning error in short distance, and can generate larger accumulated error when being used for a long time. On the other hand, certain specific actions can eliminate errors at one time, and meanwhile, known map information can be used for identifying a non-line-of-sight environment, but the map information cannot be used for target positioning alone.
Based on the current situation, various methods applied to the field of high-precision indoor positioning can be found to have certain advantages and disadvantages. How to reduce the influence of multipath effect, non-line-of-sight measurement and transmission distance on a positioning result in the positioning process, and meanwhile, the invention reduces the construction cost of application, improves the privacy of application, and makes the application more suitable for consumption level scenes, and becomes the problem to be solved by the invention.
Disclosure of Invention
In order to overcome the defects, the invention provides a TDOA-PDR-MAP fusion positioning method applied to a complex indoor space. The method makes full use of the characteristic that the positioning technology based on the acoustic signal has high positioning precision in an unobstructed indoor scene, takes TDOA position estimation based on the acoustic signal as measurement information of particle filtering, can effectively make up for accumulated errors of PDR positioning, and can assist in estimating step length information in a PDR model. Meanwhile, the PDR positioning model is used as a state equation of particle filtering by utilizing the characteristics that the PDR-based positioning technology is not easily influenced by environmental noise and multipath effect and the positioning error in a short distance is not large, so that the defect that the acoustic signal is positioned in a non-line-of-sight environment can be overcome. Meanwhile, by utilizing the position constraint based on the indoor map information, on one hand, the positioning result can be effectively corrected, on the other hand, certain specific actions can eliminate errors at one time, and meanwhile, the non-line-of-sight environment can be identified by utilizing the known map information.
The technical scheme adopted by the invention is as follows: a TDOA-PDR-MAP fusion positioning method applied to a complex indoor space comprises the following steps:
step (1), a smart phone is used as a target node, acoustic signal interaction is carried out with a fixed beacon node, TDOA measurement of an acoustic signal is obtained, and a target position is estimated by using a deviation compensation algorithm based on the TDOA measurement;
step (2), an electronic map which can be read and operated at any time is established by using a building structure and a data structure represented by mixing a vector layer and a map layer;
and (3) positioning the target by using a particle filter fusion algorithm by taking the PDR motion model as a system state equation, taking the TDOA acoustic signal positioning result as a system measurement value and taking the electronic map information as the constraint of the target motion path.
Further, the specific operation of the step (1) is as follows:
defining the target position estimate as
Figure BDA0002029125050000021
The number of the beacon nodes is nsWherein the coordinate of the ith beacon node is Si＝(sxi,syi)，nsAnd sounding the beacon nodes in turn, and selecting the beacon node with the number j as a reference node. Let the distance difference measurement value between the beacon numbered i and the reference node numbered j be Δ dijD represents the actual distance difference between the beacon node numbered i and the reference node numbered jijLet the measured noise of the distance difference between the beacon node numbered i and the reference node numbered j be epsilonij，εijCompliance
Figure BDA0002029125050000022
Zero mean gaussian distribution, σijIs the measurement noise variance associated with the environment, with Δ dij＝dij+εij(ii) a Selecting the beacon node with the number of 1 as a reference node to obtain a model of measuring the TDOA of the acoustic signal according to the relation:
Figure BDA0002029125050000023
the deviation compensation algorithm of TDOA measurement estimates the target position in the TDOA measurement model of the acoustic signal, and the algorithm comprises the following steps:
step (a 1): according to the measuring model of the TDOA of the acoustic signal, an augmentation matrix H [ -G ] is constructed1 h1]Wherein, in the step (A),
Figure BDA0002029125050000031
step (a 2): according to the measured noise, obtaining the delta H-H0Wherein, H is0The actual value of the matrix H is, and delta H is a measurement error item of the matrix H;
step (a 3): obtaining a constraint matrix Ω ═ E { Δ H) of the deviation according to the step (a2)TW1Δ H }, wherein W1To minimize systematic measurement errors, the weighting matrix is biasedWhen only the sub-matrix at the lower right corner of the poor constraint matrix omega is a non-zero matrix, recording the poor constraint matrix omega as a non-zero matrix
Figure BDA0002029125050000032
Step (a 4): h is to beTW1H is divided according to the structure of the constraint matrix Ω of the deviation in step (a3) to obtain a block matrix of 2 × 2:
Figure BDA0002029125050000033
wherein A is11、A12、
Figure BDA0002029125050000034
A22A sub-matrix which is a constraint matrix Ω of the deviations;
step (a 5): definition of v1、ν2Estimating a solution z for a preliminary target location1The estimated component, λ, is a parameter related to the signal-to-noise ratio, is solved by
Figure BDA0002029125050000035
Step (a 6): solving for ν by1：
Figure BDA0002029125050000036
Step (a 7): obtaining a preliminary target position estimation solution z according to the steps (A5) and (A6)1Wherein L is a vector v2Length of (d):
Figure BDA0002029125050000037
step (A8): solving the preliminary target position estimate of step (A7) to z1Carry over to the second position estimate solution zaWherein G isaH is a parameter matrix, ΨIs z1Error covariance matrix of (2):
Figure BDA0002029125050000038
step (a 9): using the second position estimate solution z of step (A8)aUpdating the parameter matrix and the error covariance matrix, and recording the updated parameter matrix as Ga', h ', the updated error covariance matrix Ψ ';
step (a 10): estimating the third time position estimation solution z by using weighted least squarea'：za'＝(Ga'TΨ'- 1Ga')-1Ga'TΨ'-1h'；
Step (a 11): obtaining a position estimation result:
Figure BDA0002029125050000041
further, the specific operation of the step (2) is as follows: through a data structure expressed by mixing a vector and a layer, a plurality of characteristics of space (space), line (line), point (point) and node (node) are abstracted from a building structure to form an electronic map, wherein each characteristic quantity occupies one layer in the map, the characteristic quantities can be independently called and calculated, and the characteristic quantities are connected through vector expression.
Further, the specific operation of the step (3) is as follows:
(C1) definition of Xk＝[xk yk]TFor the user's true coordinates at time k,/kIs the true step size at time k, θkIs the heading angle at time k,
Figure BDA0002029125050000042
the system state transition noise is obtained by taking a PDR motion model as a system state equation:
Figure BDA0002029125050000043
(C2) using the positioning result of the TDOA acoustic signal as a system observation equation:
Figure BDA0002029125050000044
wherein, the positioning result of the acoustic signal at the time of recording k is
Figure BDA0002029125050000045
σwIs the observed noise of the system;
(C3) initializing a particle position and a particle step size based on the TDOA position estimate;
defining the total number of particles as n, wherein the state of the ith particle at the moment of k is
Figure BDA0002029125050000046
The weight is
Figure BDA0002029125050000047
Setting an initialization time window m, the initial position of the target at the time m being the TDOA position estimation information
Figure BDA0002029125050000048
By being in an initial position
Figure BDA0002029125050000049
Adding zero mean Gaussian white noise to obtain the initialization information of the particles
Figure BDA00020291250500000410
Setting the initialization weight of each particle at m time
Figure BDA00020291250500000411
And obtaining an initial step length estimated value at the moment by using m TDOA position estimations:
Figure BDA00020291250500000412
(C4) updating the weight of the particles and estimating the target position based on the TDOA measurement information and the electronic map information;
weight of ith particle based on TDOA measurement
Figure BDA0002029125050000051
Comprises the following steps:
Figure BDA0002029125050000052
wherein
Figure BDA0002029125050000053
Weight of ith particle based on judging whether particle is transferred to completely-unconnected space
Figure BDA0002029125050000054
Comprises the following steps:
Figure BDA0002029125050000055
weight value of ith particle based on judging whether particle is transferred to current unconnected space
Figure BDA0002029125050000056
Comprises the following steps:
Figure BDA0002029125050000057
multiplying the three weights to serve as a weight updating formula of the particle:
Figure BDA0002029125050000058
then, the weight of the particles is normalized to obtain
Figure BDA0002029125050000059
Figure BDA00020291250500000510
The position estimation result of the target at the time k is as follows:
Figure BDA00020291250500000511
(C5) when number of effective particles
Figure BDA00020291250500000512
When the number of particles is smaller than the effective particle number threshold value, resampling the particles based on a random resampling mode;
(C6) updating the real step length l of the k-time particle based on the position estimation result of the target at the k time by utilizing a least square methodk：
lk＝(Al TAl)-1Al TBl
Wherein A isl＝[xk-xk-m+1 yk-yk-m+1]T，
Figure BDA00020291250500000513
(C7) The electronic map information is used for restraining and correcting the positioning result; and when the target is judged to have the door-through action at the moment k, utilizing the electronic map information to restrain the target path and updating the position estimation result.
Further, in the step (C7), when it is determined that the door-through motion of the target occurs at time k, the position estimation result is updated by restricting the target route using the electronic map information, specifically:
when the target is found to pass through the door at the moment k, returning the positioning result to N steps, namely estimating the target position at the moment k-N to be positioned at the coordinate of the door, wherein in the next N steps, the relative position of the target coordinate of each step and the target coordinate of the previous step is unchanged; defining the coordinates of the door through which the object passes as Xd＝[xd yd]TThe updated target position estimate at time k may be obtained as:
Figure BDA0002029125050000061
compared with the prior art, the invention has the following beneficial effects: the invention firstly provides a fusion positioning algorithm based on sound signal TDOA position estimation, a PDR algorithm and electronic map information, which is suitable for a complex indoor positioning scene, and solves the problems that accumulated errors are generated by PDR long-time positioning and sound signals generate extremely large positioning errors in non-line-of-sight and multi-path environments, and the positioning accuracy can reach within a meter level.
Drawings
FIG. 1 is a TDOA-PDR-MAP fusion positioning algorithm structure;
FIG. 2 is an electronic map information data structure;
FIG. 3 is a schematic diagram of a system motion model;
fig. 4 is a pedestrian motion trajectory tracking diagram.
Detailed Description
The invention provides a TDOA-PDR-MAP fusion positioning method applied to a complex indoor space, which is shown in a figure 1 and specifically comprises the following steps.
Step 1, estimating a target position by using a deviation compensation algorithm based on TDOA measurement.
In the TDOA location model, the target location estimate is defined as
Figure BDA0002029125050000062
The number of the beacon nodes is nsWherein the coordinate of the ith beacon node is Si＝(sxi,syi)，nsAnd sounding the beacon nodes in turn, and selecting the beacon node with the number j as a reference node. Let the distance difference measurement value between the beacon numbered i and the reference node numbered j be Δ dijD represents the actual distance difference between the beacon node numbered i and the reference node numbered jijLet the measured noise of the distance difference between the beacon node numbered i and the reference node numbered j be epsilonij，εijCompliance
Figure BDA0002029125050000063
Zero mean gaussian distribution, σijIs the measurement noise variance associated with the environment, with Δ dij＝dij+εij. Selecting the beacon node with the number of 1 as a reference node to obtain the acoustic signal T according to the relationDOA measurement model:
Figure BDA0002029125050000064
the system has a measurement error vector of
Figure BDA0002029125050000065
Gaussian noise epsilon assuming independent distribution of error vectorsi1～N(0,σ2) Then, the covariance matrix Q of the measured error vector is:
Figure BDA0002029125050000071
the acoustic signal TDOA measurement model is obtained by arranging: b is1n＝h1-G1z1
Wherein the parameters
Figure BDA0002029125050000072
z1For the true value of the position estimate, n is the measured noise matrix, and two parameter matrices for the system are as follows, where XiCoordinates for the ith sensor:
Figure BDA0002029125050000073
the deviation compensation algorithm of TDOA measurement estimates the target position in the TDOA measurement model of the acoustic signal, and the algorithm comprises the following steps:
step (a 1): according to the measuring model of the TDOA of the acoustic signal, an augmentation matrix H [ -G ] is constructed1 h1]；
Step (a 2): according to the measured noise, obtaining the delta H-H0Wherein, H is0The actual value of the matrix H is, and delta H is a measurement error item of the matrix H;
step (a 3): obtaining a constraint matrix Ω ═ E { Δ H) of the deviation according to the step (a2)TW1Δ H }, wherein W1To make the system measureThe weighted matrix with minimized error is recorded as the constraint matrix omega of deviation only the sub-matrix at the lower right corner is a non-zero matrix
Figure BDA0002029125050000074
Step (a 4): h is to beTW1H is divided according to the structure of the constraint matrix Ω of the deviation in step (a3) to obtain a block matrix of 2 × 2:
Figure BDA0002029125050000075
wherein A is11、A12、
Figure BDA0002029125050000076
A22A sub-matrix which is a constraint matrix Ω of the deviations;
step (a 5): definition of v1、ν2Estimating a solution z for a preliminary target location1The estimated component, λ, is a parameter related to the signal-to-noise ratio, is solved by
Figure BDA0002029125050000077
Step (a 6): solving for ν by1：
Figure BDA0002029125050000081
Step (a 7): obtaining a preliminary target position estimation solution z according to the steps (A5) and (A6)1Wherein L is a vector v2Length of (d):
Figure BDA0002029125050000082
step (A8): solving the preliminary target position estimate of step (A7) to z1Carry over to the second position estimate solution zaWherein, in the step (A),Gah is a parameter matrix, Ψ is z1Error covariance matrix of (2):
Figure BDA0002029125050000083
step (a 9): using the second position estimate solution z of step (A8)aUpdating the parameter matrix and the error covariance matrix, and recording the updated parameter matrix as Ga', h ', the updated error covariance matrix Ψ ';
step (a 10): estimating the third time position estimation solution z by using weighted least squarea'：za'＝(Ga'TΨ'- 1Ga')-1Ga'TΨ'-1h'；
Step (a 11): obtaining a position estimation result:
Figure BDA0002029125050000084
and 2, modeling the indoor electronic map by using the building structure.
Map information is constructed through a data structure expressed by mixing the vector and the map layer, so that various map information can be conveniently inquired, called and calculated in subsequent algorithm design. The method abstracts a plurality of characteristics of space (space), line (line), point (point) and node (node) from necessary map information, each characteristic quantity occupies a layer in the map, can be called and calculated independently, and the characteristic quantities are connected through vector representation. The data relationships between several features are represented as a data structure diagram as shown. The description of each characteristic amount is as follows:
(1) map (Map). Each map is made up of a collection of spaces that we consider pedestrians to be active only within a given map when making a fix.
(2) Space (space). A space is a unit constituting a map, and an environment in which pedestrians can freely move, such as a room, a corridor, and a staircase, is defined as a space. The characteristic attributes of the space include: a Space number (Space _ id), a Space connectivity (Connect), a Line set (Line) constituting a Space, and a set of nodes (nodes) included in a Space for sound signal localization.
(3) Line (line). The lines are units forming the space, and the characteristic variables of the door and the wall separating different spaces are defined as the lines, and the lines with the smallest sections are used for description. The characteristic attributes of the line include: line number (Line _ id), whether the attribute is a gate (Door), and set of start point coordinates and end point coordinates (Points).
(4) Point (point). The points are all splicing points, turning points and other manually-calibrated interest points in the space. The characteristic attributes of the points include: point number (Point _ id), abscissa (x), and ordinate (y).
(5) A node (node). The nodes are beacon nodes which are arranged in each space in advance and used for positioning the TDOA acoustic signals, and the positions of the beacon nodes are the same as map information, generally do not change for a long time after the beacon nodes are arranged, and therefore the beacon nodes are also put into a map data structure. The characteristic attributes of the node include: point number (Node _ id), abscissa (x), and ordinate (y).
And 3, positioning the target by using a TDOA-PDR-MAP particle filter fusion algorithm.
In this step, based on the framework of particle filtering, the PDR motion model is used as the system state equation, the TDOA acoustic signal location result is used as the system measurement value, and the electronic map information is used as the constraint of the target motion path. The specific algorithm steps are as follows:
(1) setting system state equation
The schematic diagram of the PDR motion model is shown in the figure, and the model is taken as a system state equation:
Figure BDA0002029125050000091
wherein, Xk＝[xk yk]TFor the user's true coordinates at time k,/kIs the true step size at that moment, θkFor the course angle at this time, the system state transition noise is zero-mean gaussian distributed: sigmac～N(0，σc 2)。
(2) And setting a system observation equation.
Using the positioning result of the TDOA acoustic signal as a system observation equation:
Figure BDA0002029125050000093
wherein the positioning result of the acoustic signal is
Figure BDA0002029125050000094
V(k)＝[σwσw]TIs the observed noise matrix, σ, of the systemwIs the observed noise of the system.
(3) The particle position and particle step size are initialized based on the TDOA position estimate.
Let the particle state space be: pi＝[xi yi]TN, where n is the total number of particles, (x)i，yi) The position coordinates of the ith particle on the x-axis and the y-axis. Setting an initialization time window m, the initial position of the target at the time m being the TDOA position estimation information
Figure BDA0002029125050000095
By being in an initial position
Figure BDA0002029125050000096
Adding zero mean Gaussian white noise to obtain the initialization information of the particles
Figure BDA0002029125050000097
And setting the initialization weight of each particle at the m moment
Figure BDA0002029125050000098
Then, the initial step length estimated value at the moment is obtained by using m TDOA position estimations:
Figure BDA0002029125050000099
(4) and updating the particle weight and estimating the target position based on the TDOA measurement information and the electronic map information.
The weight updating formula of the particles is composed of a particle weight formula based on TDOA measurement, a completely unconnected space judgment and a current unconnected space judgment.
The significance of the particle weights based on TDOA measurements is to retain those particles with reliable information by increasing the weights, and eliminate unreliable particles by decreasing the weights. The reliability of the particles is judged according to the distance between the particles and the observed value, the reliability of the particles close to the observed value is high, and the weight value of the particles is required to be increased; if the observation is far away, the weight should be reduced. Weight of ith particle based on TDOA measurement
Figure BDA0002029125050000101
Comprises the following steps:
Figure BDA0002029125050000102
wherein
Figure BDA0002029125050000103
When the line set contained in the space does not contain the line with the attribute of 'gate', the space is judged to be a completely unconnected space. Adopting a binarization judgment condition, if the coordinate falls into a completely unconnected space, directly setting the index of the particle to be 0, and judging whether the particle is transferred to the weight of the ith particle in the completely unconnected space or not based on the weight of the ith particle
Figure BDA0002029125050000104
Comprises the following steps:
Figure BDA0002029125050000105
after the ith particle is subjected to state transition once at the moment k, estimating the position of the particle at the moment k-1 and the space where the coordinates after the state transition are locatedAnd if the lines of the common 'door' attribute are not contained between the spaces, judging the space to be the current unconnected space. Adopting a binarization judgment condition, if the particle falls into the current unconnected space, directly setting the index of the particle to be 0, and judging whether the particle is transferred to the weight of the ith particle in the current unconnected space or not
Figure BDA0002029125050000106
Comprises the following steps:
Figure BDA0002029125050000107
multiplying the three weights to serve as a weight updating formula of the particle:
Figure BDA0002029125050000108
then, the particle weight is normalized to obtain:
Figure BDA0002029125050000109
the position estimation result of the target at the time k is as follows:
Figure BDA00020291250500001010
(5) and (5) resampling the particles.
When number of effective particles
Figure BDA0002029125050000111
And when the number of the particles is smaller than the effective particle number threshold value, resampling the particles based on a random resampling mode. The specific method comprises the following steps: the n particle weights after weight update are arranged at [0,1 ]]In the interval, the length of the interval occupied by each particle is equal to the weight of the particle, and the interval is called as the weight interval of the particle. Then in [0,1 ]]Taking n random numbers between intervals, finding out the particle corresponding to the interval according to the weight value interval of each random number, and copying the particle to new oneA set of particles. In the new particle set, the weight of each particle is 1/n, and the particle attribute still conforms to the distribution of the old particle set.
(6) And updating the particle step size.
Updating the real step length l of the k-time particle based on the position estimation result of the target at the k time by utilizing a least square methodk：
lk＝(Al TAl)-1Al TBl
Wherein A isl＝[xk-xk-m+1 yk-yk-m+1]T，
Figure BDA0002029125050000112
(7) And utilizing the electronic map information to restrain and correct the positioning result.
And when the target is judged to have the door-through action at the moment k, the target path is restrained by using the electronic map information. The specific method comprises the following steps: and when the target is found to pass through the door at the moment k, backing the positioning result by N steps, namely estimating the target position at the moment k-N to be positioned at the coordinates of the door, wherein in the next N steps, the relative position of the target coordinate of each step and the target coordinate of the previous step is unchanged. The corrected target position estimate at time k is obtained as:
Figure BDA0002029125050000113
the main contributions of the invention are: compared with the positioning precision of single TDOA positioning and single PDR positioning, the positioning algorithm has higher precision under the same environment. But at the same time, the positioning effect depends on the following factors in the implementation process: first, environmental noise, because the noise of specific frequency is sent by the beacon in the acoustic signal positioning process, when the noise of this frequency channel contains in the environmental noise, positioning accuracy can greatly reduced. Secondly, the accuracy of electronic map information construction, because map information is introduced into the algorithm as a constraint condition, when the map information is inaccurate, the algorithm is misjudged, and the positioning accuracy is influenced. And thirdly, the sensor accuracy of mobile devices such as mobile phones and the like, and because the PDR related measurement information in the algorithm is acquired through the personal mobile device of the user, different positioning errors can be caused by the difference of the sensor accuracy of the mobile devices.
Application environment
The invention focuses on a consumption-level positioning scene, pays attention to the improvement of positioning accuracy, considers the construction cost and the protection of user privacy, and is suitable for a complex indoor scene with known map information, capable of placing beacon nodes, initiatively initiating a positioning service request by a user and needing high-accuracy positioning. Such as shop navigation in shopping malls, automatic car finding in parking lots, player guidance in live-action games, robot binning at express delivery sites, and personnel management in enclosed spaces.

Claims (4)

1. A TDOA-PDR-MAP fusion positioning method applied to a complex indoor space is characterized by comprising the following steps:
step (1), a smart phone is used as a target node, acoustic signal interaction is carried out with a fixed beacon node, TDOA measurement of an acoustic signal is obtained, and a target position is estimated by using a deviation compensation algorithm based on the TDOA measurement;
step (2), an electronic map which can be read and operated at any time is established by using a building structure and a data structure represented by mixing a vector layer and a map layer;
step (3), a PDR motion model is used as a system state equation, a TDOA acoustic signal positioning result is used as a system measurement value, electronic map information is used as the constraint of a target motion path, and a particle filter fusion algorithm is used for positioning a target;
wherein the specific operation of the step (1) is as follows:
defining the target position estimate as
Figure FDA0002706776800000011
The number of the beacon nodes is nsWherein the coordinate of the ith beacon node is Si＝(sxi,syi)，nsIndividual beacon sectionSounding the points in turn, and selecting the beacon node with the number j as a reference node; let the distance difference measurement value between the beacon numbered i and the reference node numbered j be Δ dijD represents the actual distance difference between the beacon node numbered i and the reference node numbered jijLet the measured noise of the distance difference between the beacon node numbered i and the reference node numbered j be epsilonij，εijCompliance
Figure FDA0002706776800000012
Zero mean gaussian distribution, σijIs the measurement noise variance associated with the environment, with Δ dij＝dij+εij(ii) a Selecting the beacon node with the number of 1 as a reference node to obtain a model of measuring the TDOA of the acoustic signal according to the relation:
Figure FDA0002706776800000013
the deviation compensation algorithm of TDOA measurement estimates the target position in the TDOA measurement model of the acoustic signal, and the algorithm comprises the following steps:
step (a 1): according to the measuring model of the TDOA of the acoustic signal, an augmentation matrix H [ -G ] is constructed1 h1]Wherein, in the step (A),
Figure FDA0002706776800000014
step (a 2): according to the measured noise, obtaining the delta H-H0Wherein, H is0The actual value of the matrix H is, and delta H is a measurement error item of the matrix H;
step (a 3): obtaining a constraint matrix Ω ═ E { Δ H) of the deviation according to the step (a2)TW1Δ H }, wherein W1To minimize the systematic measurement error, the weight matrix is recorded as the constraint matrix Ω of the deviation, when only the submatrix at the bottom right corner is a non-zero matrix
Figure FDA0002706776800000021
Step (a 4): h is to beTW1H is divided according to the structure of the constraint matrix Ω of the deviation in step (a3) to obtain a block matrix of 2 × 2:
Figure FDA0002706776800000022
wherein A is11、A12、
Figure FDA0002706776800000023
A22A sub-matrix which is a constraint matrix Ω of the deviations;
step (a 5): definition of v1、ν2Estimating a solution z for a preliminary target location1The estimated component, λ, is a parameter related to the signal-to-noise ratio, is solved by
Figure FDA0002706776800000024
Step (a 6): solving for ν by1：
Figure FDA0002706776800000025
Step (a 7): obtaining a preliminary target position estimation solution z according to the steps (A5) and (A6)1Wherein L is a vector v2Length of (d):
Figure FDA0002706776800000026
step (A8): solving the preliminary target position estimate of step (A7) to z1Carry over to the second position estimate solution zaWherein G isaH is a parameter matrix, Ψ is z1Error covariance matrix of (2):
Figure FDA0002706776800000027
step (a 9): using the second position estimate solution z of step (A8)aUpdating the parameter matrix and the error covariance matrix, and recording the updated parameter matrix as Ga', h ', the updated error covariance matrix Ψ ';
step (a 10): estimating the third time position estimation solution z by using weighted least squarea'：
za'＝(Ga'TΨ'-1Ga')-1Ga'TΨ'-1h'
Step (a 11): obtaining a position estimation result:
Figure FDA0002706776800000028
or
Figure FDA0002706776800000029
2. the method according to claim 1, characterized in that the specific operation of step (2) is: through a data structure expressed by mixing a vector and a layer, a plurality of characteristics of space (space), line (line), point (point) and node (node) are abstracted from a building structure to form an electronic map, wherein each characteristic quantity occupies one layer in the map, the characteristic quantities can be independently called and calculated, and the characteristic quantities are connected through vector expression.
3. The method according to claim 1, characterized in that the specific operation of step (3) is:
(C1) definition of Xk＝[xk yk]TFor the user's true coordinates at time k,/kIs the true step size at time k, θkCourse angle at time k, σc～N(0,σc 2) Is system state transition noise, takes PDR motion model as the systemThe state equation is as follows:
Figure FDA0002706776800000031
(C2) using the positioning result of the TDOA acoustic signal as a system observation equation:
Figure FDA0002706776800000032
wherein, the positioning result of the acoustic signal at the time of recording k is
Figure FDA0002706776800000033
σwIs the observed noise of the system;
(C3) initializing a particle position and a particle step size based on the TDOA position estimate;
defining the total number of particles as n, wherein the state of the ith particle at the moment of k is
Figure FDA0002706776800000034
The weight is
Figure FDA0002706776800000035
Setting an initialization time window m, the initial position of the target at the time m being the TDOA position estimation information
Figure FDA0002706776800000036
By being in an initial position
Figure FDA0002706776800000037
Adding zero mean Gaussian white noise to obtain the initialization information of the particles
Figure FDA0002706776800000038
Setting the initialization weight of each particle at m time
Figure FDA0002706776800000039
And obtaining the initial time of the moment by using m TDOA position estimatesInitial step length estimation:
Figure FDA00027067768000000310
(C4) updating the weight of the particles and estimating the target position based on the TDOA measurement information and the electronic map information;
weight of ith particle based on TDOA measurement
Figure FDA00027067768000000311
Comprises the following steps:
Figure FDA00027067768000000312
wherein
Figure FDA00027067768000000313
Weight of ith particle based on judging whether particle is transferred to completely-unconnected space
Figure FDA00027067768000000314
Comprises the following steps:
Figure FDA00027067768000000315
weight value of ith particle based on judging whether particle is transferred to current unconnected space
Figure FDA00027067768000000316
Comprises the following steps:
Figure FDA00027067768000000317
multiplying the three weights to serve as a weight updating formula of the particle:
Figure FDA00027067768000000318
then, the weight of the particles is normalized to obtain
Figure FDA00027067768000000319
Figure FDA0002706776800000041
The position estimation result of the target at the time k is as follows:
Figure FDA0002706776800000042
(C5) when number of effective particles
Figure FDA0002706776800000043
When the number of particles is smaller than the effective particle number threshold value, resampling the particles based on a random resampling mode;
(C6) updating the real step length l of the k-time particle based on the position estimation result of the target at the k time by utilizing a least square methodk：
lk＝(Al TAl)-1Al TBl
Wherein A isl＝[xk-xk-m+1 yk-yk-m+1]T，
Figure FDA0002706776800000044
(C7) The electronic map information is used for restraining and correcting the positioning result; and when the target is judged to have the door-through action at the moment k, utilizing the electronic map information to restrain the target path and updating the position estimation result.
4. The method according to claim 1, wherein in the step (C7), when it is determined that the target performs the door-through action at time k, the electronic map information is used to constrain the target path and update the position estimation result, and the method specifically comprises: when the target is found to pass through the door at the moment k, the positioning result is backed by N steps, namely the target position estimation at the moment k-N is positioned at the coordinates of the door, and in the next N steps, the target of each stepThe relative position of the target coordinate and the target coordinate of the previous step is unchanged; defining the coordinates of the door through which the object passes as Xd＝[xd yd]TThe updated target position estimate at time k may be obtained as:
Figure FDA0002706776800000045
CN201910303807.5A 2019-04-16 2019-04-16 TDOA-PDR-MAP fusion positioning method applied to complex indoor space Active CN110187306B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910303807.5A CN110187306B (en) 2019-04-16 2019-04-16 TDOA-PDR-MAP fusion positioning method applied to complex indoor space

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910303807.5A CN110187306B (en) 2019-04-16 2019-04-16 TDOA-PDR-MAP fusion positioning method applied to complex indoor space

Publications (2)

Publication Number Publication Date
CN110187306A CN110187306A (en) 2019-08-30
CN110187306B true CN110187306B (en) 2021-01-08

Family

ID=67714576

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910303807.5A Active CN110187306B (en) 2019-04-16 2019-04-16 TDOA-PDR-MAP fusion positioning method applied to complex indoor space

Country Status (1)

Country Link
CN (1) CN110187306B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111024075B (en) * 2019-12-26 2022-04-12 北京航天控制仪器研究所 Pedestrian navigation error correction filtering method combining Bluetooth beacon and map
CN111486843B (en) * 2020-04-03 2022-01-11 深圳市百慕大工业有限公司 Positioning method, device and positioning equipment in complex environment

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103528587A (en) * 2013-10-15 2014-01-22 西北工业大学 Autonomous integrated navigation system
CN104076382A (en) * 2014-07-22 2014-10-01 中国石油大学(华东) Seamless vehicle positioning method based on multi-source information fusion
CN107193279A (en) * 2017-05-09 2017-09-22 复旦大学 Robot localization and map structuring system based on monocular vision and IMU information

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8600660B2 (en) * 2006-09-29 2013-12-03 Honeywell International Inc. Multipath modeling for deep integration
CN105115487B (en) * 2015-05-18 2017-09-01 陈希 Positioning navigation method in a kind of supermarket based on information fusion
CN105652243B (en) * 2016-03-14 2017-12-05 西南科技大学 Multichannel group sparse linear predicts delay time estimation method
CN106093858B (en) * 2016-06-22 2019-02-05 山东大学 A kind of positioning system and localization method based on UWB, RFID, INS multi-source alignment by union technology
CN108151747B (en) * 2017-12-27 2021-05-04 浙江大学 Indoor positioning system and positioning method using fusion of acoustic signals and inertial navigation

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103528587A (en) * 2013-10-15 2014-01-22 西北工业大学 Autonomous integrated navigation system
CN104076382A (en) * 2014-07-22 2014-10-01 中国石油大学(华东) Seamless vehicle positioning method based on multi-source information fusion
CN107193279A (en) * 2017-05-09 2017-09-22 复旦大学 Robot localization and map structuring system based on monocular vision and IMU information

Also Published As

Publication number Publication date
CN110187306A (en) 2019-08-30

Similar Documents

Publication Publication Date Title
Liu et al. Kalman filter-based data fusion of Wi-Fi RTT and PDR for indoor localization
US11968591B2 (en) Maintenance of a location fingerprint database for an area
Zhuang et al. Autonomous smartphone-based WiFi positioning system by using access points localization and crowdsourcing
CN104781686B (en) Pathway matching
CN110856106B (en) Indoor high-precision three-dimensional positioning method based on UWB and barometer
CN106912105B (en) Three-dimensional positioning method based on PSO _ BP neural network
CN109413578B (en) Indoor positioning method based on fusion of WIFI and PDR
CN113074739A (en) UWB/INS fusion positioning method based on dynamic robust volume Kalman
CN109655786B (en) Mobile ad hoc network cooperation relative positioning method and device
CN109379711B (en) positioning method
US20170123044A1 (en) Method of determining position and cooperative positioning system using same
CN110187306B (en) TDOA-PDR-MAP fusion positioning method applied to complex indoor space
CN111148030A (en) Fingerprint database updating method and device, server and storage medium
CN108566677B (en) Fingerprint positioning method and device
CN116567531A (en) Sensor fusion indoor positioning method and system based on particle filter algorithm
CN103596265A (en) Multiple-user indoor positioning method based on voice distance measuring and movement vector
WO2014055845A1 (en) Speed estimation using delta rtt measurements and area maps
US9258679B1 (en) Modifying a history of geographic locations of a computing device
Chen et al. An optimal selection of sensors in multi-sensor fusion navigation with factor graph
US20150211845A1 (en) Methods and Systems for Applying Weights to Information From Correlated Measurements for Likelihood Formulations Based on Time or Position Density
US20230044777A1 (en) Developing a fingerprint map for determining an indoor location of a wireless device
KR20140119333A (en) Method and Apparatus for Location Determination to Improve the accuracy of the location
CN110595464A (en) IMU and visual sensor fusion positioning method and device
Kessel et al. Automated WLAN calibration with a backtracking particle filter
Wang et al. Barometer assisted smartphone localization for vehicle navigation in multilayer road networks

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant