CN114111802B - Pedestrian track reckoning auxiliary UWB positioning method - Google Patents
Pedestrian track reckoning auxiliary UWB positioning method Download PDFInfo
- Publication number
- CN114111802B CN114111802B CN202111573062.8A CN202111573062A CN114111802B CN 114111802 B CN114111802 B CN 114111802B CN 202111573062 A CN202111573062 A CN 202111573062A CN 114111802 B CN114111802 B CN 114111802B
- Authority
- CN
- China
- Prior art keywords
- uwb
- positioning
- particles
- particle
- mobile module
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 59
- 239000002245 particle Substances 0.000 claims abstract description 97
- 230000008859 change Effects 0.000 claims abstract description 10
- 230000004927 fusion Effects 0.000 claims abstract description 10
- 238000001914 filtration Methods 0.000 claims abstract description 7
- 230000033001 locomotion Effects 0.000 claims description 16
- 239000013598 vector Substances 0.000 claims description 16
- 238000009826 distribution Methods 0.000 claims description 11
- 238000012952 Resampling Methods 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000010606 normalization Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 3
- 238000009827 uniform distribution Methods 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 description 5
- 238000002474 experimental method Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 206010033799 Paralysis Diseases 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000003595 spectral effect Effects 0.000 description 1
- 238000012549 training Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- 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/06—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
-
- 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/024—Guidance services
-
- 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/025—Services making use of location information using location based information parameters
- H04W4/027—Services making use of location information using location based information parameters using movement velocity, acceleration information
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention provides a pedestrian dead reckoning auxiliary UWB positioning method, which utilizes pedestrian dead reckoning and a distance observation value between a mobile module and two UWB base station nodes to carry out fusion positioning by adopting a particle filtering method, namely, utilizes the two UWB base station nodes and a pedestrian dead reckoning PDR to realize stable combined positioning. The advantages of autonomy of PDR positioning, insensitivity to environmental change, high-precision positioning and high-precision ranging performance of the UWB positioning system are utilized, and the advantages of two positioning modes are fully utilized; the normal positioning function of the UWB positioning system can be ensured under the condition that part of UWB base station nodes are blocked by walls or other obstacles, so that the expansibility of the UWB positioning system is improved, and the cost for realizing large-scale positioning tasks in a large scale and arranging base station nodes in a large quantity can be reduced.
Description
Technical Field
The invention relates to the technical field of indoor positioning, in particular to a positioning method of pedestrian dead reckoning auxiliary UWB.
Background
Location services are services based on obtaining accurate locations, which mainly include both outdoor and indoor locations. The global positioning and navigation system (global navigation satellite system, GNSS) mainly comprises a GPS in the United states, a Glonass in Russian, a Galileo in Europe and a Beidou satellite navigation system in China, wherein the GPS is taken as a main outdoor positioning and navigation technology which is very wide in the civil field, covers the aspects of social life and becomes an indispensable important technology in daily life of people. In recent years, beidou satellite navigation systems are continuously developed and are increasingly widely applied. Research shows that 80% of people are in indoor environment, GNSS positioning can not extend to indoor environment due to attenuation of electromagnetic wave signals, indoor location service has huge commercial potential and wide application prospect, such as market, airport, exhibition hall, underground engineering and other indoor environment requirements, and compared with outdoor positioning, the indoor environment space and complexity face a small challenge to indoor positioning technology. Through many years of research and development, various indoor positioning technical schemes such as Bluetooth, wi-Fi, zigBee, inertial navigation, UWB and the like are proposed and applied, and the positioning accuracy, cost investment and use scene of each positioning mode are different.
UWB positioning is a kind of very high precision in the indoor positioning technology at present, UWB signal can modulate to UWB work frequency band through frequency conversion modes such as differentiation or mixing through narrow pulse (such as secondary Gaussian pulse) that transmission time is very short (such as 2 ns), has advantages such as high spectral range, high time resolution, anti multipath performance are strong. The method is characterized in that the coordinates of a mobile node are calculated by measuring the signal arrival Time between the mobile node and the base station node and converting the signal arrival Time into the distance, and using TOA (Time of arrival) or TDOA (Time Difference 0 fArrival) algorithm, the UWB positioning system needs to be provided with a sufficient number (at least three) of base station nodes (UWB transceivers installed at fixed positions), the system installation and operation cost is high, the system is easily influenced by non-line-of-sight (not line of sight, NLOS) factors, and when necessary base station node data is absent, the positioning system falls into paralysis, and the expansibility of the UWB positioning system is limited.
The indoor positioning mode based on the inertia measurement element refers to the estimation of the PDR (Pedestrian Dead Reckoning, PDR) of the pedestrian dead reckoning, and the basic principle is that the acceleration and angular velocity data in the pedestrian movement process are measured in real time through the inertia sensors such as the accelerometer, the gyroscope and the magnetometer which are integrated based on the MEMS technology, and the position coordinates in the movement process are estimated through the estimation of the step frequency, the step length and the direction by the algorithm. PDR is a completely autonomous positioning mode, does not need to arrange infrastructure, and has low positioning cost, but accumulated errors of sensor drift exist, short-term precision is high, and errors are increased as time passes.
In the research of the existing fusion positioning algorithm, aiming at the fusion positioning of PDR and UWB, a complete UWB positioning system is mainly used for correcting the positioning error of the PDR, when the UWB positioning system fails, the PDR is used for positioning independently, the positioning error can be accumulated gradually, and the combined positioning is meaningless.
In view of the foregoing, a positioning method of pedestrian dead reckoning assisted UWB is urgently needed to solve the problems in the prior art.
Disclosure of Invention
The invention aims to provide a pedestrian dead reckoning auxiliary UWB positioning method, which aims to solve the problems that when a conventional PDR and UWB fusion positioning algorithm encounters non-line-of-sight influence (namely, less than three UWB base station nodes in communication with a mobile node), combined positioning errors are gradually accumulated and combined positioning is meaningless, and the specific technical scheme is as follows:
a pedestrian dead reckoning auxiliary UWB positioning method uses pedestrian dead reckoning and distance observation values between a mobile module and two UWB base station nodes to carry out fusion positioning by adopting a particle filtering method, and comprises the following specific steps:
step S1: particle initialization, and generating initial particle groups P with uniform distribution and N quantity around the initial coordinates of the mobile module according to Gaussian distribution 0 ；
Step S2: the movement module starts to move, and the system state X of the particles at the t-th step is predicted by using a prediction model t j ；
Step S3: by two distance observations r t 1 and rt 2 Solving the probability of the distance observation value of each particle in the t-th step state to update the weight for the particle, and carrying out normalization processing;
step S4: obtaining a new particle state of the t-th step after the processing of step S2 and step S3Set P t ；
Step S5: and resampling the particles, and resolving the position coordinates of the mobile module by using the particle set after resampling the particles.
In the above technical solution, preferably, the mobile module includes a UWB mobile node and an inertial sensor; the inertial sensor comprises an accelerometer, a gyroscope and a magnetometer; the accelerometer acquires the linear velocity change of the mobile module in real time, and the gyroscope and the magnetometer acquire the angular velocity change of the mobile module in real time.
In the above technical solution, preferably, the UWB mobile node of the mobile module communicates with two UWB base station nodes in the open, and obtains the distance observation value r between the mobile module and the two UWB base station nodes by two-way ranging method t 1 and rt 2 The method comprises the steps of carrying out a first treatment on the surface of the According to the data acquired by the inertial sensor in the mobile module, the step length l of the t-th step in the motion is obtained by utilizing the dead reckoning of the pedestrian t And heading alpha t 。
In the above technical solution, the particle initialization in step S1 preferably includes the following two methods:
the first is: initial coordinates of the mobile module (x 0 ,y 0 ) From its initial state X when known 0 ＝[x 0 ,y 0 ,0,0]According to Gaussian distribution
Generating uniformly distributed particles, wherein->
An initial state vector representing the jth particle, N being the total number of particles, σ coor Standard deviation for initial coordinate determination;
the second is: when the size of the region in the room is known, the particles are initialized based on the geometric relationship of the region.
In the above technical scheme, preferably, the initial particle group P in the step S1 0 The method comprises the following steps:
wherein ,
an initial state vector representing the jth particle, N being the total number of particles, the initial weight of the particles being
In the above technical solution, in step S2, the prediction model is preferably:
wherein, the system state of the t-1 step is that
x t-1 、y t-1 Representing the coordinates of the t-1 step movement module; alpha t-1 、l t-1 Respectively representing the heading and step length of the movement at the t-1 th step,/for each step>
For the state transition matrix of the predictive model, +.>
As an input matrix of the prediction model, delta alpha is zero-mean Gaussian random noise in course estimation, and delta l is zero-mean Gaussian random noise in step estimation.
In the above technical scheme, preferably, Δα and Δl respectively conform to
Distribution, wherein sigma α For medium error in course angle solving, sigma l Is the medium error of the step size estimation.
In the above technical solution, in the step S3, two UWB base station nodes and a mobile station are preferably selectedThe ranging vector of the module is O t ＝[r t 1 ,r t 2 ] T Distance measurement between two UWB base station nodes is independent, namely probability
and />
Independent of each other, the distance measurement vector O corresponding to the particles at the t-th step t The probability of (2) is:
the ranging probability corresponding to the jth individual particle may be expressed as:
wherein ,
expressed as the coordinates, sigma, of the jth individual particle at the t-th step r Standard deviation for UWB ranging, +.>
and />
Coordinates of UWB base station nodes, where i=1, 2;
updating the weight of the particles in the t step by the weight of the particles in the t-1 step:
and carrying out normalization treatment on the particles:
in the above technical solution, in step S5, the position coordinates of the mobile module are calculated by using a weighted average of the system state vectors of all the particles, where the weighted average is:
the technical scheme of the invention has the following beneficial effects:
the positioning method of the invention realizes stable combined positioning by only utilizing Two UWB base stations (TUA) and pedestrian dead reckoning PDR, not only can ensure the normal positioning function of the UWB positioning system under the condition that part of UWB base station nodes are blocked by walls or other obstacles, thereby improving the expansibility of the UWB positioning system, but also reducing the cost generated by arranging a large amount of base station nodes for realizing large-scale positioning tasks.
The advantages of autonomy of PDR positioning, insensitivity to environmental change, high-precision positioning and high-precision ranging performance of the UWB positioning system are utilized, and the advantages of two positioning modes are fully utilized, so that the layout of UWB positioning base stations can be reduced and the positioning cost can be reduced particularly in a complex environment. The positioning method can realize the fusion of PDR and lack of UWB positioning of the base station, and is used as a fault-tolerant alternative scheme when the UWB positioning system fails (the number of the base stations is insufficient and coordinate calculation cannot be completed).
According to the positioning method, the step length and the heading of each step are obtained by utilizing the PDR, the randomness of the movement of a target (person) is fully considered, the state of the target, namely the step length and the change state of the movement direction, are estimated in real time through the sensor information acquired in real time, the independence of state variable estimation at each moment is realized, the actual situation is fully considered, and the positioning is more accurate.
According to the positioning method, the influence of errors of the inertial sensor in the PDR on the course estimation and the step estimation is fully considered, and Gaussian noise of the course estimation and the step estimation is introduced. The noise parameters can be obtained by training in advance, so that the estimation of the error model is more accurate, the application range is wider, and the method also has the advantages of high positioning precision, small accumulated error, high availability and the like.
In addition to the objects, features and advantages described above, the present invention has other objects, features and advantages. The present invention will be described in further detail with reference to the drawings.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the invention. In the drawings:
FIG. 1 is a schematic diagram of a positioning method of the present invention;
FIG. 2 is a schematic diagram of the positioning method of the present invention;
FIG. 3 is a flow chart diagram of a positioning method of the present invention;
FIG. 4 is a graph showing the accuracy of the positioning method of the present invention compared with the accuracy of the conventional positioning method.
Detailed Description
The present invention will be described more fully hereinafter in order to facilitate an understanding of the present invention, and preferred embodiments of the present invention are set forth. This invention may, however, be embodied in many different forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention.
Example 1:
referring to fig. 1, four UWB base stations distributed in space can communicate with a mobile module without shielding, and coordinates of the UWB base stations can be specified in a custom reference coordinate system to obtain distance data between any three UWB base stations and the mobile module, so that the coordinates of the mobile module can be resolved in real time through trilateral positioning. However, in the actual positioning, when the mobile module moves to the t-th step, the situation that only two UWB base station nodes communicate with the mobile module can occur, and at the moment, the coordinates of the mobile module cannot be calculated through trilateral positioning.
Preferably, the mobile module comprises a UWB mobile node and an inertial sensor; the inertial sensor comprises an accelerometer, a gyroscope and a magnetometer; the accelerometer acquires the linear velocity change of the mobile module in real time, and the gyroscope and the magnetometer acquire the angular velocity change of the mobile module in real time. As can be seen from the equation (1), in PDR (pedestrian dead reckoning), the initial coordinates (x t-1 ,y t-1 ) Knowing the step length l is obtained t And heading alpha t The coordinates of each step can be recursively derived.
However, since there is a fixed offset when the inertial sensor measures data, the positioning error of the PDR accumulated over time is increased and a divergence phenomenon occurs, so that the PDR cannot obtain accurate positioning information.
Preferably, referring to fig. 2, the positioning method of the present embodiment uses a particle filtering method to perform fusion positioning by using pedestrian dead reckoning and distance observation values between the mobile module and Two UWB base station nodes, that is, uses Two UWB base station nodes (TUA) and pedestrian dead reckoning PDR to implement stable combined positioning. The advantages of autonomy of PDR positioning, insensitivity to environmental change, high-precision positioning and high-precision ranging performance of the UWB positioning system are utilized, and the advantages of two positioning modes are fully utilized, so that the layout of UWB positioning base stations can be reduced and the positioning cost can be reduced particularly in a complex environment.
In this embodiment, the step length l of the t-th step in motion is obtained by using the PDR according to the data acquired by the inertial sensor in the mobile module t And heading alpha t . Wherein step length l t According to the data of the accelerometer, the algorithm such as zero-speed update, nonlinear relation and the like can be used for resolving the heading alpha t The method is generally combined with 9-axis data of an accelerometer, a gyroscope and a magnetometer to carry out calculation, and the main methods are a complementary filtering method, a gradient descent method and a Kalman filtering method.
Further preferably, the UWB mobile node of the mobile module communicates with two UWB base station nodes in the sight, the coordinate of the UWB base station nodes is
Measuring the flight time of electromagnetic wave signals between modules by two-way distance measurement method and multiplying the flight time by the speed of electromagnetic waves (light speed) to obtain the distance observation value r between the mobile module and two UWB base station nodes t 1 and rt 2 The method comprises the steps of carrying out a first treatment on the surface of the And then the particle filter algorithm is used for filtering the particles t 、α t 、r t 1 、r t 2 The position coordinates of the mobile module are fused and calculated, as shown in fig. 3, and the specific steps are as follows:
step S1: particle initialization, and generating initial particle groups P with uniform distribution and N quantity around the initial coordinates of the mobile module according to Gaussian distribution 0 (wherein the coordinates of the mobile module are specified in a custom reference coordinate system);
preferably, to implement the particle filter fusion algorithm, a set of particles needs to be initialized first, and the particle initialization in step S1 includes the following two methods:
the first is: initial coordinates of the mobile module (x 0 ,y 0 ) When known, from the initial state X 0 ＝[x 0 ,y 0 ,0,0]According to Gaussian distribution
Generating uniformly distributed particles, wherein->
An initial state vector representing the jth particle, N being the total number of particles, σ coor For the beginningStandard deviation of initial coordinate determination.
The second is: when the size of the region in the room is known, the particles are initialized based on the geometric relationship of the region. Long range like known area (room) x Wide range y Information can be generated
Preferably, the initial particle group P in the step S1 0 The method comprises the following steps:
wherein ,
an initial state vector representing the jth particle with an initial value of +.>
N is the total number of particles, the initial weight of the particles is +.>
Step S2: the hand-held mobile module of the pedestrian starts to step (note: after the mobile module starts to move, the particle state representing the position of the mobile module moves along with the movement), and the system state of the particle at the t-th step is predicted by using a prediction model
Preferably, in the step S2, the prediction model is:
wherein, the system state of the t-1 step is that
x t-1 、y t-1 Representing the coordinates of the t-1 step movement module; alpha t-1 、l t-1 Respectively representing the heading and step length of the movement at the t-1 th step,/for each step>
For the state transition matrix of the predictive model, +.>
As an input matrix of the prediction model, delta alpha is zero-mean Gaussian random noise in course estimation, and delta l is zero-mean Gaussian random noise in step estimation; Δα and Δl correspond to respectively
Distribution, wherein sigma α For medium error in course angle solving, sigma l Sigma, the medium error of step estimation α 、σ l Can be measured or set as an empirical value
The two most critical parameters in the PDR positioning are heading estimation and step estimation, and the data received by the sensor is affected by random noise during walking, and the random noise delta alpha and delta l versus alpha are fully considered in the PDR positioning in the embodiment t 、l t Is favorable for improving the positioning accuracy.
Step S3: by two distance observations r t 1 and rt 2 Solving the probability of the distance observation value of each particle in the t-th step state to update the weight for the particle, and carrying out normalization processing;
specifically, in the step S3, the ranging vectors of the two UWB base station nodes and the mobile module are O t ＝[r t 1 ,r t 2 ] T Distance measurement between two UWB base station nodes is independent, namely probability
and />
Independent of each other, the distance measurement vector O corresponding to the particles at the t-th step t The probability of (2) is:
the ranging probability corresponding to the jth individual particle may be expressed as:
wherein ,
expressed as the coordinates, sigma, of the jth individual particle at the t-th step r Standard deviation for UWB ranging, +.>
and />
Coordinates of UWB base station nodes, where i=1, 2; />
Is a standard formula of normal distribution in mathematics;
updating the weight of the particles in the t step by the weight of the particles in the t-1 step:
and carrying out normalization treatment on the particles:
step S4: obtaining a new set of particle states P of step t after the processing of step S2 and step S3 t ：
Step S5: and resampling the particles, and resolving the position coordinates of the mobile module by using the particle set after resampling the particles.
Particle resampling, also known as particle importance sampling, is a process for modifying the distribution of particles in a state space. In the new particle set, the larger the particle weight is, the closer the part of particles are to the real position of the mobile module, the resampling is carried out on the particles, the particles with low weight are removed, the particles with high weight are added, but the total number of the particles is kept unchanged, so that the particle set is closer to the real position.
Each particle has coordinates, and the estimated value of the coordinates of the mobile module can be obtained after solving the state vectors of all the particles, so that the positioning is completed, specifically, in step S5, the position coordinates of the mobile module are solved by using the weighted average value of the system state vectors of all the particles, where the weighted average value is:
experimental comparison:
experiment setting: referring to fig. 4, two UWB base station nodes are installed C, D in an indoor environment, the coordinates of which are known. The experimenter holds the mobile module to walk along the planned real path GD from the starting Point SP (Start Point) at the right lower corner in the figure to carry out a positioning experiment, and the sight between the mobile module and two UWB base station nodes is kept to be free from shielding in the experimental process.
Referring to fig. 4, tua-PDR represents a positioning track of the positioning method of the present embodiment, and it can be seen that the positioning method of the present embodiment better fits the real path GD; the nonL-LS represents the positioning trace of the nonlinear least squares method (i.e., the algorithm based on the fusion of UWB+PDR of the nonL-LS) in the experiment, and the positioning effect is worse than that of the method of the present embodiment. PDR represents a positioning track positioned by adopting pure PDR, the positioning errors of the method are gradually accumulated, the positioning track is gradually diverged, and the positioning effect is poorer than that of the method of the embodiment; the UWB represents the positioning track adopting pure UWB positioning, the positioning error of the method is smaller than that of the positioning method of the embodiment, but the comparison of the positioning track of the embodiment, the pure UWB positioning track and the real path GD shows that the embodiment not only fits the real path GD well, but also can obtain the positioning precision close to the pure UWB, thus the method of the embodiment can ensure the positioning precision after the UWB positioning is invalid.
Conclusion of experiment: through the positioning experiment in the indoor environment, the positioning method of the embodiment can ensure the positioning precision after the UWB positioning system fails, can also reduce the cost of additionally arranging UWB base station nodes, and remarkably improves the applicability of the UWB positioning system in the complex environment.
The above description is only of the preferred embodiments of the present invention and is not intended to limit the present invention, but various modifications and variations can be made to the present invention by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (8)
1. The pedestrian dead reckoning auxiliary UWB positioning method is characterized in that pedestrian dead reckoning and a distance observation value between a mobile module and two UWB base station nodes are used for fusion positioning by adopting a particle filtering method, and the method comprises the following specific steps:
step S1: particle initialization, and generating initial particle groups P with uniform distribution and N quantity around the initial coordinates of the mobile module according to Gaussian distribution 0 ；
Step S2: the movement module starts to move, and the system state of the particles at the t-th step is predicted by using a prediction model
Step S3: by two distance observations
and />
Solving the probability of the distance observation value of each particle in the t-th step state to update the weight of the particle, and carrying out normalization processing;
step S4: obtaining a new set of particle states P of step t after the processing of step S2 and step S3 t ；
Step S5: resampling the particles, and resolving the position coordinates of the mobile module by using a particle set obtained after resampling the particles;
in the step S2, the prediction model is:
wherein, the system state of the t-1 step is that
x t-1 、y t-1 Representing the coordinates of the t-1 step movement module; alpha t-1 、l t-1 Respectively representing the heading and step length of the movement at the t-1 th step,/for each step>
For the state transition matrix of the predictive model, +.>
As an input matrix of the prediction model, delta alpha is zero-mean Gaussian random noise in course estimation, and delta l is zero-mean Gaussian random noise in step estimation.
2. The pedestrian dead reckoning aided UWB positioning method of claim 1 wherein the mobile module includes a UWB mobile node and an inertial sensor; the inertial sensor comprises an accelerometer, a gyroscope and a magnetometer; the accelerometer acquires the linear velocity change of the mobile module in real time, and the gyroscope and the magnetometer acquire the angular velocity change of the mobile module in real time.
3. The pedestrian dead reckoning aided UWB positioning method of claim 2 wherein the UWB mobile node of the mobile module communicates with two UWB base station nodes in the view, and distance observations between the mobile module and the two UWB base station nodes are obtained by two-way ranging
and />
According to the data acquired by the inertial sensor in the mobile module, the step length l of the t-th step in the motion is obtained by utilizing the dead reckoning of the pedestrian t And heading alpha t 。
4. The positioning method of the pedestrian dead reckoning aided UWB according to claim 1, wherein the particle initialization in the step S1 comprises the following two methods:
the first is: initial coordinates of the mobile module (x 0 ,y 0 ) From its initial state X when known 0 ＝[x 0 ,y 0 ,0,0]According to Gaussian distribution
Generating uniformly distributed particles, wherein->
An initial state vector representing the jth particle, N being the total number of particles, σ coor Standard deviation for initial coordinate determination;
the second is: when the size of the region in the room is known, the particles are initialized based on the geometric relationship of the region.
5. The pedestrian dead reckoning aided UWB positioning method of claim 4 wherein the initial particle set P in step S1 0 The method comprises the following steps:
wherein ,
an initial state vector representing the jth particle, N being the total number of particles, the initial weight of the particles being +.>
6. The pedestrian dead reckoning aided UWB positioning method of claim 1 wherein Δα and Δl correspond to, respectively
Distribution, wherein sigma α For medium error in course angle solving, sigma l Is the medium error of the step size estimation.
7. The method for positioning a pedestrian dead reckoning aided UWB according to claim 1, wherein in step S3, the ranging vectors of the two UWB base station nodes and the mobile module are
Distance measurement between two UWB base station nodes is independent of each other, i.e. probability +.>
and />
Independent of each other, then the particles at the t-th step correspond toDistance measurement vector O t The probability of (2) is:
the ranging probability corresponding to the j-th particle individual is expressed as:
wherein ,
expressed as the coordinates, sigma, of the jth individual particle at the t-th step r Standard deviation for UWB ranging, +.>
and />
Coordinates of UWB base station nodes, where i=1, 2;
updating the weight of the particles in the t step by the weight of the particles in the t-1 step:
and carrying out normalization treatment on the particles:
8. the method for positioning a pedestrian dead reckoning aided UWB according to claim 1, wherein in step S5, the position coordinates of the mobile module are calculated by using a weighted average of the system state vectors of all particles, and the weighted average is:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111573062.8A CN114111802B (en) | 2021-12-21 | 2021-12-21 | Pedestrian track reckoning auxiliary UWB positioning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111573062.8A CN114111802B (en) | 2021-12-21 | 2021-12-21 | Pedestrian track reckoning auxiliary UWB positioning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114111802A CN114111802A (en) | 2022-03-01 |
| CN114111802B true CN114111802B (en) | 2023-06-13 |
Family
ID=80362481
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111573062.8A Active CN114111802B (en) | 2021-12-21 | 2021-12-21 | Pedestrian track reckoning auxiliary UWB positioning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114111802B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114894196B (en) * | 2022-05-20 | 2023-09-19 | 上海航天测控通信研究所 | Dead reckoning-assisted departure angle direction finding and positioning method |
| CN115348539B (en) * | 2022-08-15 | 2023-10-10 | 南京濠暻通讯科技有限公司 | Underground communication and positioning integrated system based on 5G+ |
| CN117490689A (en) * | 2023-11-06 | 2024-02-02 | 河南理工大学 | Two-dimensional ultra-wideband and inertial navigation fusion positioning method and system |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109916410B (en) * | 2019-03-25 | 2023-04-28 | 南京理工大学 | Indoor positioning method based on improved square root unscented Kalman filtering |
| CN110375730B (en) * | 2019-06-12 | 2021-07-27 | 深圳大学 | Indoor positioning navigation system based on IMU and UWB fusion |
| CN110426040A (en) * | 2019-07-08 | 2019-11-08 | 中国人民解放军陆军工程大学 | Indoor pedestrian positioning method with non-line-of-sight identification function |
| CN111256695B (en) * | 2020-01-14 | 2022-09-09 | 电子科技大学 | UWB/INS combined indoor positioning method based on particle filter algorithm |
-
2021
- 2021-12-21 CN CN202111573062.8A patent/CN114111802B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN114111802A (en) | 2022-03-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114111802B (en) | Pedestrian track reckoning auxiliary UWB positioning method | |
| Zhuang et al. | Tightly-coupled integration of WiFi and MEMS sensors on handheld devices for indoor pedestrian navigation | |
| Zhuang et al. | Multi-sensor integrated navigation/positioning systems using data fusion: From analytics-based to learning-based approaches | |
| Fan et al. | Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB | |
| US20070282565A1 (en) | Object locating in restricted environments using personal navigation | |
| CN112923919B (en) | Pedestrian positioning method and system based on graph optimization | |
| Yu et al. | Precise 3D indoor localization and trajectory optimization based on sparse Wi-Fi FTM anchors and built-in sensors | |
| CN103471586A (en) | Sensor-assisted terminal combination positioning method and sensor-assisted terminal combination positioning device | |
| CN116182867A (en) | INS/UWB unmanned aerial vehicle positioning method based on tight combination in complex indoor environment | |
| CN116047567B (en) | Deep learning assistance-based guard and inertial navigation combined positioning method and navigation method | |
| Zhou et al. | Wi-Fi RTT/encoder/INS-based robot indoor localization using smartphones | |
| Xia et al. | A novel PDR aided UWB indoor positioning method | |
| Zhang et al. | Using UWB aided GNSS/INS integrated navigation to bridge GNSS outages based on optimal anchor distribution strategy | |
| CN105043389A (en) | Single external illuminator-based combined navigation method | |
| Yang et al. | Indoor mobile localization based on a tightly coupled UWB/INS integration | |
| van Graas et al. | GPS/INS-a very different way | |
| Lategahn et al. | Robust pedestrian localization in indoor environments with an IMU aided TDoA system | |
| Elbes et al. | Gyroscope drift correction based on TDoA technology in support of pedestrian dead reckoning | |
| Zhang et al. | A multi-pedestrian cooperative navigation and positioning method based on UWB technology | |
| Asano et al. | A robust pedestrian dead-reckoning positioning based on pedestrian behavior and sensor validity | |
| Li et al. | Variance optimization of UWB observation based on map matching for PPP/INS/UWB tightly coupled positioning | |
| Runnalls et al. | Terrain-referenced navigation using the IGMAP data fusion algorithm | |
| Retscher | An intelligent multi-sensor system for pedestrian navigation | |
| Yu et al. | Indoor mobile robot positioning based on uwb and low cost imu | |
| Vana | Continuous urban navigation with next-generation, mass market navigation sensors and adaptive filtering |
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 |