CN116448111A - Pedestrian indoor navigation method, device and medium based on multi-source information fusion - Google Patents
Pedestrian indoor navigation method, device and medium based on multi-source information fusion Download PDFInfo
- Publication number
- CN116448111A CN116448111A CN202310210607.1A CN202310210607A CN116448111A CN 116448111 A CN116448111 A CN 116448111A CN 202310210607 A CN202310210607 A CN 202310210607A CN 116448111 A CN116448111 A CN 116448111A
- Authority
- CN
- China
- Prior art keywords
- pedestrian
- state
- matrix
- information
- source information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 84
- 230000004927 fusion Effects 0.000 title claims abstract description 49
- 239000002245 particle Substances 0.000 claims abstract description 71
- 238000001914 filtration Methods 0.000 claims abstract description 61
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 28
- 238000001514 detection method Methods 0.000 claims abstract description 9
- 230000005021 gait Effects 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 57
- 230000001133 acceleration Effects 0.000 claims description 30
- 230000006870 function Effects 0.000 claims description 26
- 230000008569 process Effects 0.000 claims description 15
- 239000013598 vector Substances 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 9
- 238000012952 Resampling Methods 0.000 claims description 8
- 238000010606 normalization Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 5
- 238000005259 measurement Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 8
- 238000007499 fusion processing Methods 0.000 abstract description 2
- 230000009286 beneficial effect Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000010276 construction Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000006467 substitution reaction Methods 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 238000004590 computer program Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000001627 detrimental effect Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000000644 propagated effect Effects 0.000 description 1
Classifications
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H17/00—Networks using digital techniques
- H03H17/02—Frequency selective networks
- H03H17/0248—Filters characterised by a particular frequency response or filtering method
- H03H17/0255—Filters based on statistics
- H03H17/0257—KALMAN filters
-
- 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
- G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
- G01S11/02—Systems for determining distance or velocity not using reflection or reradiation using radio waves
- G01S11/06—Systems for determining distance or velocity not using reflection or reradiation using radio waves using intensity measurements
-
- 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)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Probability & Statistics with Applications (AREA)
- Computer Hardware Design (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention discloses a pedestrian indoor navigation method, a device and a medium based on multi-source information fusion, wherein the method comprises the following steps: map information of the current indoor environment is built, and a Wi-Fi fingerprint library is built; acquiring multi-source information; constructing a position model of the pedestrian to obtain position information of the pedestrian; obtaining the movement distance of the pedestrian through gait detection and step estimation; constructing a fusion model based on extended Kalman filtering, and carrying out fusion processing on multi-source information and pedestrian position information to obtain fused position information; and further correcting and updating the position information of the pedestrians according to the map information and the particle filtering algorithm by utilizing the fused position information. According to the invention, a multisource information fusion technology is introduced, an extended Kalman filtering algorithm and a particle filtering algorithm are introduced, wi-Fi fingerprint matching and map information are added, a proper model is created, errors in an inertial navigation system are corrected, and safer, more reliable and more accurate position information is provided for the system.
Description
Technical Field
The invention relates to the technical field of navigation, in particular to a pedestrian indoor navigation method, device and medium based on multi-source information fusion.
Background
With the development and progress of society, the living standard of people is increasingly improved, and the demand for reliable and real-time location-based services is also increasingly high. However, under indoor conditions, the problems of more obstacles, more interference sources and the like exist due to complex and changeable indoor environments, so that the satellite navigation signal positioning performance is poor, and the effect in indoor positioning is poor. When satellite navigation fails, common navigation methods include:
(1) Inertial navigation, the indoor positioning technology based on the inertial device is inertial navigation based on an inertial sensor and mainly comprises a pedestrian inertial navigation system PINS and a pedestrian dead reckoning PDR. The former often requires a high accuracy of the sensor and requires a long-term positioning reliability by suppressing error drift; the latter often has lower accuracy requirements on the sensor, and depending on the characteristics of the pedestrian, the pedestrian position is updated in a step-by-step accumulation manner in the pedestrian navigation direction through stride detection. PDR tends to achieve better positioning results and higher cost effectiveness than PINS under cost-limited conditions. However, since low cost MEMS inertial sensors tend to have large errors and the errors accumulate gradually over time, this has a very detrimental effect on the positioning results.
(2) Wi-Fi positioning is popular in recent years, most of indoor Wi-Fi signal coverage exists, and although a large number of existing infrastructures are provided for supporting the Wi-Fi signal-based positioning technology, wi-Fi signal strength can be directly obtained through a smart phone, so that the development of the Wi-Fi positioning technology is promoted to a great extent, but the positioning accuracy is generally difficult to guarantee because the Wi-Fi signal is easily interfered by the environment.
(3) The multi-source information fusion technology utilizes information acquired by a plurality of sensors to classify, process, fuse and use the information according to specific rules, so as to realize information advantage complementation among the sensors. Complementary with the inertial navigation advantages, the inertial navigation accumulated error is effectively restrained, and the positioning accuracy is further improved.
Along with the increasing demands of people on indoor high-precision positioning, the positioning precision demands are also improved, but a technical scheme of indoor high-precision positioning is still lacking.
Disclosure of Invention
In order to solve at least one of the technical problems in the prior art to a certain extent, the invention aims to provide a pedestrian indoor navigation method, device and medium based on multi-source information fusion.
The technical scheme adopted by the invention is as follows:
a pedestrian indoor navigation method based on multi-source information fusion comprises the following steps:
constructing map information of the current indoor environment, selecting a reference point on a map, and constructing a Wi-Fi fingerprint library;
acquiring multi-source information by using a sensor on an intelligent terminal, wherein the multi-source information comprises Wi-Fi signal strength, angular velocity, speed, acceleration and gesture information;
constructing a pedestrian position model according to the Wi-Fi signal intensity and the Wi-Fi fingerprint library to obtain pedestrian position information;
judging the instantaneous gesture of the intelligent terminal through the gesture information, converting the acceleration from a carrier coordinate system to a navigation coordinate system, and obtaining the movement distance of the pedestrian through gait detection and step estimation;
according to the multisource information and the movement distance, a movement model of the pedestrian is constructed, and movement information of the pedestrian is obtained, wherein the movement information comprises: position, speed; according to an extended Kalman filtering algorithm, a fusion model based on the extended Kalman filtering is constructed, multi-source information and pedestrian position information are fused, fused position information is obtained, and state vector updating and optimal estimation of error values in the extended Kalman filtering are completed;
and further correcting and updating the pedestrian position information according to the map information and the particle filter algorithm by utilizing the fused position information, so as to complete the pedestrian indoor navigation with multi-source information fusion.
Further, the expression of the position model of the pedestrian is:
wherein,,represents the estimated position, x (t k )、y(t k ) Representing the position coordinates, ω, corresponding to the first K position points j (t k ) Weight coefficients representing the corresponding data points, the weight coefficients based on signal strength may represent +.>RSSI is the signal strength.
Further, the acceleration is translated from the carrier coordinate system to the navigation coordinate system in the following manner:
wherein,,for navigating a directional cosine matrix from the navigation system to the carrier system, C r C for rotating matrix around Y axis p For a rotation matrix about the Z-axis, C y For a rotation matrix about the X-axis, γ is the rotation angle about the Y-axis, p is the rotation angle about the X-axis, and Y is the rotation angle about the Z-axis.
Further, the step of gait detection includes:
calculating the overall acceleration:
wherein a is x ，a y ，a z For the collected triaxial acceleration value, the influence of the sensor posture is reduced by calculating the overall acceleration a;
according to the overall acceleration, a potential peak value is obtained in the sliding window, and primary judgment is carried out by utilizing a preset acceleration threshold value;
calculating the time difference between the potential wave crest and the previous wave crest, and performing secondary judgment by using a preset walking one-step time threshold range;
and comparing the potential peak with the front and rear neighborhood acceleration, judging for three times to remove the pseudo peak, if the potential peak point is the maximum value, recording one step by the algorithm, otherwise, not performing step counting processing.
Further, the step size is estimated as:
where SL is the step size, a max And a min The maximum and minimum of the longitudinal acceleration of the pedestrian, respectively.
Further, the extended kalman filter algorithm includes:
X k+1 ＝f(X k )+w k
wherein X is k+1 X is the state variable of the (k+1) th step k Is the state variable of the kth step, w k Is process noise; f () is a nonlinear state function in extended kalman filtering;
the method for realizing the linearization of the system matrix comprises the following steps:
wherein Z is k To observe the variable, v k H () is a measurement function in extended kalman filtering for observing noise;
the method for realizing the linearization of the observation matrix comprises the following steps:
wherein H is k Is a jacobian matrix of h (X) to X bias,and (5) estimating the state of the kth step.
Further, the step of expanding the state vector update in the kalman filter includes:
one-step state prediction update:
in the method, in the process of the invention,is the optimal value of the last state, +.>A one-step predicted value for the current state;
one-step predictive estimation error covariance matrix update:
wherein P is k+1|k Is thatCorresponding covariance one-step predictor, phi k+1|k For state transition matrix, P k|k Is->Corresponding covariance,/>Is phi k+1|k Transpose of Q k A process noise covariance matrix;
calculating an extended Kalman filtering gain:
wherein K is k+1 To expand the Kalman filter gain matrix, H k+1 In order to observe the matrix,for transpose of the observation matrix, R k+1 Measuring a noise covariance matrix;
calculating information from the observation vectors and updating the state estimate:
in the method, in the process of the invention,z is the optimal estimated value of the current state k+1 For the current observation +.>The current observation predicted value; updating the estimated error covariance:
P k+1 ＝[I-K k+1 H k+1 ]P k+1|k
wherein P is k+1 Is thatCorresponding covariance, I is the identity matrix.
Further, the further correcting and updating the position information of the pedestrian according to the map information and the particle filtering algorithm comprises the following steps:
the state transfer equation and the observation equation for particle filtering are as follows:
x(t)＝f(x(t-1),u(t),w(t))
y(t)＝f(x(t),e(t))
wherein x (t) is a state at time t, u (t) is a control quantity, w (t) and e (t) are state noise and observation noise respectively, and particle filtering filters the state at time t x (t) from observation y (t) and the state at the last time x (t-1), u (t) and w (t);
the updating method in particle filtering mainly comprises the following steps:
a1, initializing: initial value x of observed value i (k) As an initial value of the probability density function x (0), byFor a pair ofSampling N times, initial w i (k) Set to->
A2, one-step prediction: for each particle x i (k) By converting the formula p (x (k+1) |x i (k) Obtaining a new particle;
a3, importance sampling: for any particle x i (k+1) solving for their weights w i (k+1)＝p(z(k+1)|x i (k+1))；
A4, normalization: and (3) carrying out normalization processing on the weight values:
a5, resampling: resampling the particles according to the weight, wherein the probability of repeating the particles with the weight is high, and the probability of repeating the particles with the weight is low;
a6, updating particles and simultaneously adding map information:
because of the limit value of the indoor walking area, map matching is performed according to map information, and when the one-step predicted particle passes through the passable area (such as wall crossing), the weight value of the particle is given as 0:
and A7, calculating by adopting a basic particle filtering algorithm, and completing the indoor positioning and navigation of the pedestrians according to the calculation result.
The invention adopts another technical scheme that:
a pedestrian indoor navigation device based on multi-source information fusion, comprising:
at least one processor;
at least one memory for storing at least one program;
the at least one program, when executed by the at least one processor, causes the at least one processor to implement the method described above.
The invention adopts another technical scheme that:
a computer readable storage medium, in which a processor executable program is stored, which when executed by a processor is adapted to carry out the method as described above.
The beneficial effects of the invention are as follows: according to the invention, a multisource information fusion technology is introduced, an extended Kalman filtering algorithm and a particle filtering algorithm are introduced, wi-Fi fingerprint matching and map information are added, a proper model is created, errors in an inertial navigation system are corrected, and safer, more reliable and more accurate position information is provided for the system.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following description is made with reference to the accompanying drawings of the embodiments of the present invention or the related technical solutions in the prior art, and it should be understood that the drawings in the following description are only for convenience and clarity of describing some embodiments in the technical solutions of the present invention, and other drawings may be obtained according to these drawings without the need of inventive labor for those skilled in the art.
FIG. 1 is a general flow diagram of a pedestrian indoor navigation method based on multi-source information fusion in an embodiment of the invention;
FIG. 2 is a schematic diagram of a process for constructing an extended Kalman filter fusion model in an embodiment of the invention;
fig. 3 is a schematic diagram of a particle filter model construction flow in an embodiment of the invention.
Detailed Description
Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to like or similar elements or elements having like or similar functions throughout. The embodiments described below by referring to the drawings are illustrative only and are not to be construed as limiting the invention. The step numbers in the following embodiments are set for convenience of illustration only, and the order between the steps is not limited in any way, and the execution order of the steps in the embodiments may be adaptively adjusted according to the understanding of those skilled in the art.
In the description of the present invention, it should be understood that references to orientation descriptions such as upper, lower, front, rear, left, right, etc. are based on the orientation or positional relationship shown in the drawings, are merely for convenience of description of the present invention and to simplify the description, and do not indicate or imply that the apparatus or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and thus should not be construed as limiting the present invention.
In the description of the present invention, a number means one or more, a number means two or more, and greater than, less than, exceeding, etc. are understood to not include the present number, and above, below, within, etc. are understood to include the present number. The description of the first and second is for the purpose of distinguishing between technical features only and should not be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated or implicitly indicating the precedence of the technical features indicated.
In the description of the present invention, unless explicitly defined otherwise, terms such as arrangement, installation, connection, etc. should be construed broadly and the specific meaning of the terms in the present invention can be reasonably determined by a person skilled in the art in combination with the specific contents of the technical scheme.
According to the invention, a multisource information fusion technology is introduced, an extended Kalman filtering algorithm and a particle filtering algorithm are introduced, wi-Fi fingerprint matching and map information are added, a proper model is created, errors in an inertial navigation system are corrected, and safer, more reliable and more accurate position information is provided for the system. Therefore, a pedestrian indoor navigation method based on multi-source information fusion is established, and an effective method is provided for realizing accurate positioning of a pedestrian indoor.
Referring to fig. 1, the embodiment provides a pedestrian indoor navigation method based on multi-source information fusion, which includes the following steps:
step 1: constructing map information of the current indoor environment, selecting a reference point on a map, and constructing a Wi-Fi fingerprint library; the method comprises the steps of obtaining multi-source information by using a built-in sensor of a mobile phone, wherein the multi-source information comprises: wi-Fi signal strength, angular velocity, speed, magnetic field strength, and attitude information.
Step 2: processing Wi-Fi signals acquired in the step 1, and constructing a pedestrian position model; and obtaining the position information of the pedestrians.
In the present embodiment, when the pedestrian position model is constructed, the pedestrian position model therein is improved, and the weighted k-nearest neighbor model is used instead of the k-nearest neighbor model.
Specifically, constructing the pedestrian position model includes:
wherein the method comprises the steps ofRepresents the estimated position, x (t k )、y(t k ) Representing the position coordinates, ω, corresponding to the first K position points j (t k ) Weight coefficients representing the corresponding data points, the weight coefficients based on signal strength may represent +.>RSSI is the signal strength.
Step 3: and (3) judging the instantaneous gesture of the mobile phone through the direction sensor in the step (1), and converting the acceleration from a carrier coordinate system into a navigation coordinate system through a certain method. And then the movement distance of the pedestrian is obtained through gait detection and step estimation.
The method for converting the acceleration coordinate system comprises the following steps:
wherein, defining a direction cosine matrix from the navigation system to the carrier system by rotating the navigation system n around the Z axis (positive axis) by Y angle (course angle, yaw), rotating the navigation system n around the X axis by p angle (pitch angle, pitch), and finally rotating the navigation system n around the Y axis by gamma angle (roll angle, roll)And the direction cosine matrix from the carrier to the navigation system>
Gait detection includes:
1) The overall acceleration is calculated, and the influence of the sensor posture is reduced by calculating the overall acceleration a. Wherein a is x ，a y ，a z Is the three-axis acceleration value acquired.
2) And obtaining a potential peak value in the sliding window, and performing primary judgment by using the acceleration threshold value [1.2g,3g ].
3) And calculating the time difference between the potential wave crest and the previous wave crest, and performing secondary judgment by using the walking one-step time threshold range [0.4s,1s ].
4) And comparing the potential peak with the front and rear neighborhood acceleration, judging for three times to remove the pseudo peak, if the potential peak point is the maximum value, recording one step by the algorithm, otherwise, not performing step counting processing.
The step size estimate is:
where SL is the step size, a max And a min The maximum and minimum of the longitudinal acceleration of the pedestrian, respectively.
Step 4: processing the multisource information acquired in the step 1, and constructing a pedestrian motion model; obtaining pedestrian movement information, including: position, speed; constructing a fusion model based on extended Kalman filtering according to an extended Kalman filtering algorithm; the multisource information obtained in the step 1 and the pedestrian position information obtained in the step 2 are subjected to fusion processing, and fused position information is obtained; and (5) updating the state vector and optimally estimating the error value in the extended Kalman filtering.
The extended kalman filter algorithm includes:
the state equation is:
X(k+1)＝φX(k)+ΓW(k)
wherein X (k) = [ N (k) E (k) v n (k)v e (k)] T N (k) and E (k) are the position state values in the north direction and in the east direction at time k, v n (k) And v e (k) The speed state values in the north direction and the east direction at the moment k are respectively, W (k) is the system noise at the moment k, and phi and Γ are the state transition matrix and the system noise coefficient matrix from the moment k to the moment k+1 respectively.
The observation equation is:
Z(k+1)＝H(k+1)X(k+1)+V
wherein Z (k) = [ N wifi E wifi s] T ，N wifi And E is wifi The north coordinate value and the east coordinate value are obtained through Wi-Fi respectively, and s is a plane displacement value in two seconds obtained through gait detection. H (k+1) is a measurement matrix, which can be obtained by deriving and linearizing the formula. V is the observed noise and it is assumed that the noise follows a gaussian distribution.
A method of state vector updating in extended kalman filtering, comprising the steps of:
step 4-1, one-step state prediction update:
wherein,,is the optimal value of the last state, +.>A one-step predicted value for the current state;
step 4-2, updating a one-step prediction estimation error covariance matrix:
wherein P is k+1|k Is thatCorresponding covariance one-step predictor, phi k+1|k For state transition matrix, P k|k Is->Corresponding covariance,/>Is phi k+1|k Transpose of Q k A process noise covariance matrix;
step 4-3, calculating an extended Kalman filtering gain:
wherein K is k+1 To expand the Kalman filter gain matrix, H k+1 In order to observe the matrix,for transpose of the observation matrix, R k+1 Measuring a noise covariance matrix;
step 4-4, calculating information by the observation vector and updating the state estimation:
wherein,,z is the optimal estimated value of the current state k+1 For the current observation +.>The predicted value is the current observation. Step 4-5, updating the estimated error covariance:
P k+1 ＝[I-K k+1 H k+1 ]P k+1|k
wherein P is k+1 Is thatCorresponding covariance, I is the identity matrix.
Step 5: and (3) utilizing the position information based on the extended Kalman filtering obtained in the step (4), adding map matching according to a particle filtering algorithm, and further correcting and updating the position information of the pedestrian to finish the pedestrian indoor navigation with multi-source information fusion.
The particle filter algorithm includes:
the particle filter is a direct estimation of the approximate state posterior probability density function with random samples:
wherein w is i (k) Is particle x i k The sum of all weights is 1, delta () is the dirac function.
The updating method in the particle filtering comprises the following steps:
and 5-1, initializing.
Initial value x of observed value i (k) As an initial value of the probability density function x (0), byFor->Sampling N times, initial w i (k) Set to->
And 5-2, predicting in one step.
For each particle x i (k) By converting the formula p (x (k+1) |x i (k) Obtaining a new particle x) i (k+1)。
And 5-3, importance sampling.
For any particle x i (k+1) solving for their weights w i (k+1)＝p(z(k+1)|x i (k+1))。
And 5-4, normalizing.
And carrying out normalization processing on the weight.
Step 5-5, resampling.
And resampling the particles according to the weight, wherein the probability of the particles with the weight is larger, and the probability of the particles with the weight is smaller.
And 5-6, updating particles.
Order theIs->
And 5-7, calculating by adopting a basic particle filtering algorithm.
And finishing the indoor navigation of the pedestrian by using the calculation result.
The above method is explained in detail below with reference to the drawings and specific examples.
As shown in fig. 1, the present embodiment provides a pedestrian indoor navigation method based on multi-source information fusion, and the specific method technical points are as follows:
s1, providing a Wi-Fi positioning position model building function.
All sensor information in the navigation system can be acquired through a built-in sensor in the mobile phone, and a pedestrian position model is constructed through selecting position and signal intensity information.
The key to the multi-source information fusion technique is to determine the relationship between the multi-source information. In a navigation system, the multi-source information mainly comprises three-axis angular velocity, acceleration, wi-Fi signal intensity, magnetic field intensity and the like. The construction of the pedestrian position model specifically comprises the following steps:
wherein the method comprises the steps ofRepresents the estimated position, x (t k )、y(t k ) Representing the position coordinates, ω, corresponding to the first K position points j (t k ) Weight coefficients representing the corresponding data points, the weight coefficients based on signal strength may represent +.>RSSI is the signal strength.
S2, expanding a Kalman filtering fusion model construction function.
And (3) utilizing the established model, and calculating a real-time position result by fusing information such as position, speed and the like through an extended Kalman filtering algorithm.
The conventional kalman filter is only suitable for a linear system, and the extended kalman filter is one of the kalman filter algorithms, and is to apply the conventional kalman filter to a nonlinear field, wherein a system matrix F and an observation matrix H in the conventional kalman filter are replaced by F (x) and H (x), and then the nonlinear system is linearized according to a first-order taylor expansion.
The system model of the extended kalman filter is:
X k+1 ＝f(X k )+w k
wherein X is k+1 X is the state variable of the (k+1) th step k Is the state variable of the kth step, w k Is process noise; f () is a nonlinear state function in extended kalman filtering.
The method for realizing the linearization of the system matrix comprises the following steps:
wherein F is k-1 Is a Jacobian matrix of f vs. X bias,for the posterior state estimation at step k-1, X is a state variable.
The observation model of the extended kalman filter is:
Z k ＝h(X k )+v k
wherein Z is k To observe the variable, v k H () is a measurement function in extended kalman filtering for observing noise;
the method for realizing the linearization of the observation matrix comprises the following steps:
wherein H is k Is a jacobian matrix of h (X) to X bias,and (5) estimating the state of the kth step.
As shown in fig. 2, fig. 2 shows the process of constructing the extended kalman filter fusion model of the present invention. In this embodiment, an error model is constructed by using multisource information such as angular velocity, acceleration, position, etc., and the specific extended kalman filter update mainly includes the following five steps:
step 1, one-step state prediction updating:
wherein,,is the optimal value of the last state, +.>A one-step predicted value for the current state;
step 2, updating a one-step prediction estimation error covariance matrix:
wherein P is k+1|k Is thatCorresponding covariance one-step predictor, phi k+1|k For state transition matrix, P k|k Is->Corresponding covariance,/>Is phi k+1|k Transpose of Q k A process noise covariance matrix;
step 3, calculating an extended Kalman filtering gain:
wherein K is k+1 To expand the Kalman filter gain matrix, H k+1 In order to observe the matrix,for transpose of the observation matrix, R k+1 Measuring a noise covariance matrix;
step 4, calculating information by the observation vector, and updating state estimation:
wherein,,z is the optimal estimated value of the current state k+1 For the current observation +.>The predicted value is the current observation.
Step 5, updating the estimated error covariance:
P k+1 ＝[I-K k+1 H k+1 ]P k+1|k
wherein P is k+1 Is thatCorresponding covariance, I is the identity matrix.
Based on the above five steps, the updating of the state vector and the optimal estimation of the error value in the extended Kalman filtering are completed.
S3, constructing a function based on the fusion model of particle filtering and map matching.
Based on the estimation result of the model, particle filtering is performed on the basis of expanding a Kalman filtering fusion model, and map information is added, so that the operation amount is reduced, the operation speed is increased, the error is reduced, and the model precision is improved.
The idea of particle filtering compared to kalman filtering is based on the monte carlo method, which uses particle sets to represent probabilities, which can be used on any form of state space model. The core idea is to express the distribution of random state particles extracted from posterior probability, which is a sequential importance sampling method. In short, the particle filtering method is a process of obtaining a state minimum variance distribution by approximating a probability density function by searching a group of random samples propagated in a state space and replacing an integral operation with a sample mean. The samples are particles, and the probability density distribution of any form can be approximated when the number of the samples is N-infinity.
The state transfer equation and the observation equation for particle filtering are as follows:
x(t)＝f(x(t-1),u(t),w(t))
y(t)＝f(x(t),e(t))
wherein x (t) is a t moment state, u (t) is a control quantity, w (t) and e (t) are state noise and observation noise respectively, and particle filtering filters the t moment state x (t) from the observation y (t) and the last moment state x (t-1), u (t) and w (t).
Referring to fig. 3, the updating method in the particle filtering mainly includes the following steps:
and step 1, initializing.
Initial value x of observed value i (k) As an initial value of the probability density function x (0), byFor->Sampling N times, initial w i (k) Set to->
And 2, one-step prediction.
For each particle x i (k) By converting the formula p (x (k+1) |x i (k) Obtaining a new particle x) i (k+1)。
And 3, importance sampling.
For any particle x i (k+1) solving for their weights w i (k+1)＝p(z(k+1)|x i (k+1))。
And 4, normalizing.
And carrying out normalization processing on the weight.
And 5, resampling.
And resampling the particles according to the weight, wherein the probability of the particles with the weight is larger, and the probability of the particles with the weight is smaller.
And 6, updating the particles and simultaneously adding map information.
Order theIs->
Because of the limit value of the indoor walking area, map matching is performed according to map information, and when the one-step predicted particle passes through the passable area (such as wall crossing), the weight value of the particle is given as 0:
and 7, calculating by adopting a basic particle filtering algorithm.
S4, a pedestrian indoor navigation method based on multi-source information fusion.
After the three models are constructed, the pedestrian indoor navigation method based on multi-source information fusion is basically generated. When satellite navigation fails, the navigation model is introduced, the prediction error is generated, the positioning error caused by various factors to the inertial navigation system is corrected in real time, and the positioning precision of indoor pedestrians is improved.
After the Wi-Fi positioning position model, the extended Kalman filtering fusion model and the particle filtering and map matching fusion model are constructed, the pedestrian indoor navigation method based on multi-source information fusion is basically completed, and a specific flow chart is shown in figure 1. In the pedestrian indoor navigation system, an extended Kalman filtering fusion model and a particle filtering and map matching fusion model are built by utilizing multi-source information, when the conventional satellite navigation fails, information required by the navigation system is obtained by utilizing a built-in sensor in a mobile phone, and then the pedestrian indoor navigation is carried out by utilizing the navigation method to obtain the current position information of the pedestrian.
In summary, compared with the prior art, the method of the embodiment has at least the following advantages and beneficial effects:
(1) According to the method, the traditional PDR positioning is improved by introducing the expanded Kalman filtering and the particle filtering, and the map matching is added, so that the operation amount is reduced.
(2) The pedestrian indoor navigation method based on multi-source information fusion can improve the pedestrian navigation positioning efficiency and accuracy.
(3) The model and the method in the method of the embodiment can be applied to most pedestrian indoor navigation systems.
The embodiment also provides a pedestrian indoor navigation device based on multi-source information fusion, which comprises:
at least one processor;
at least one memory for storing at least one program;
the at least one program, when executed by the at least one processor, causes the at least one processor to implement the method described above.
The pedestrian indoor navigation device based on the multi-source information fusion can execute any combination implementation steps of the pedestrian indoor navigation method based on the multi-source information fusion, and has corresponding functions and beneficial effects.
The present application also discloses a computer program product or a computer program comprising computer instructions stored in a computer readable storage medium. The computer instructions may be read from a computer-readable storage medium by a processor of a computer device, and executed by the processor, to cause the computer device to perform the method shown in fig. 1.
The embodiment also provides a storage medium which stores instructions or programs capable of executing the pedestrian indoor navigation method based on the multi-source information fusion, and when the instructions or programs are run, the random combination implementation steps of the executable method embodiment have the corresponding functions and beneficial effects of the method.
In some alternative embodiments, the functions/acts noted in the block diagrams may occur out of the order noted in the operational illustrations. For example, two blocks shown in succession may in fact be executed substantially concurrently or the blocks may sometimes be executed in the reverse order, depending upon the functionality/acts involved. Furthermore, the embodiments presented and described in the flowcharts of the present invention are provided by way of example in order to provide a more thorough understanding of the technology. The disclosed methods are not limited to the operations and logic flows presented herein. Alternative embodiments are contemplated in which the order of various operations is changed, and in which sub-operations described as part of a larger operation are performed independently.
Furthermore, while the invention is described in the context of functional modules, it should be appreciated that, unless otherwise indicated, one or more of the described functions and/or features may be integrated in a single physical device and/or software module or one or more functions and/or features may be implemented in separate physical devices or software modules. It will also be appreciated that a detailed discussion of the actual implementation of each module is not necessary to an understanding of the present invention. Rather, the actual implementation of the various functional modules in the apparatus disclosed herein will be apparent to those skilled in the art from consideration of their attributes, functions and internal relationships. Accordingly, one of ordinary skill in the art can implement the invention as set forth in the claims without undue experimentation. It is also to be understood that the specific concepts disclosed are merely illustrative and are not intended to be limiting upon the scope of the invention, which is to be defined in the appended claims and their full scope of equivalents.
The functions, if implemented in the form of software functional units and sold or used as a stand-alone product, may be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention may be embodied essentially or in a part contributing to the prior art or in a part of the technical solution, in the form of a software product stored in a storage medium, comprising several instructions for causing a computer device (which may be a personal computer, a server, a network device, etc.) to perform all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
Logic and/or steps represented in the flowcharts or otherwise described herein, e.g., a ordered listing of executable instructions for implementing logical functions, can be embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, processor-containing system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions. For the purposes of this description, a "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device.
More specific examples (a non-exhaustive list) of the computer-readable medium would include the following: an electrical connection (electronic device) having one or more wires, a portable computer diskette (magnetic device), a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). In addition, the computer readable medium may even be paper or other suitable medium on which the program is printed, as the program may be electronically captured, via, for instance, optical scanning of the paper or other medium, then compiled, interpreted or otherwise processed in a suitable manner, if necessary, and then stored in a computer memory.
It is to be understood that portions of the present invention may be implemented in hardware, software, firmware, or a combination thereof. In the above-described embodiments, the various steps or methods may be implemented in software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, may be implemented using any one or combination of the following techniques, as is well known in the art: discrete logic circuits having logic gates for implementing logic functions on data signals, application specific integrated circuits having suitable combinational logic gates, programmable Gate Arrays (PGAs), field Programmable Gate Arrays (FPGAs), and the like.
In the foregoing description of the present specification, reference has been made to the terms "one embodiment/example", "another embodiment/example", "certain embodiments/examples", and the like, means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiments or examples. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
While embodiments of the present invention have been shown and described, it will be understood by those of ordinary skill in the art that: many changes, modifications, substitutions and variations may be made to the embodiments without departing from the spirit and principles of the invention, the scope of which is defined by the claims and their equivalents.
While the preferred embodiment of the present invention has been described in detail, the present invention is not limited to the above embodiments, and various equivalent modifications and substitutions can be made by those skilled in the art without departing from the spirit of the present invention, and these equivalent modifications and substitutions are intended to be included in the scope of the present invention as defined in the appended claims.
Claims (10)
1. The pedestrian indoor navigation method based on multi-source information fusion is characterized by comprising the following steps of:
constructing map information of the current indoor environment, selecting a reference point on a map, and constructing a Wi-Fi fingerprint library;
acquiring multi-source information by using a sensor on an intelligent terminal, wherein the multi-source information comprises Wi-Fi signal strength, angular velocity, speed, acceleration and gesture information;
constructing a pedestrian position model according to the Wi-Fi signal intensity and the Wi-Fi fingerprint library to obtain pedestrian position information; judging the instantaneous gesture of the intelligent terminal through the gesture information, converting the acceleration from a carrier coordinate system to a navigation coordinate system, and obtaining the movement distance of the pedestrian through gait detection and step estimation;
constructing a motion model of the pedestrian according to the multi-source information and the motion distance to obtain motion information of the pedestrian; according to an extended Kalman filtering algorithm, a fusion model based on the extended Kalman filtering is constructed, multi-source information and pedestrian position information are fused, fused position information is obtained, and state vector updating and optimal estimation of error values in the extended Kalman filtering are completed;
and further correcting and updating the pedestrian position information according to the map information and the particle filter algorithm by utilizing the fused position information, so as to complete the pedestrian indoor navigation with multi-source information fusion.
2. The pedestrian indoor navigation method based on multi-source information fusion according to claim 1, wherein the expression of the pedestrian position model is:
wherein,,represents the estimated position, x (t k )、y(t k ) Representing the position coordinates, ω, corresponding to the first K position points j (t k ) Weight coefficients representing the corresponding data points, the weight coefficients based on signal strength may represent +.>RSSI is the signal strength.
3. The pedestrian indoor navigation method based on multi-source information fusion according to claim 1, wherein the acceleration is converted from the carrier coordinate system to the navigation coordinate system by:
wherein,,for navigating a directional cosine matrix from the navigation system to the carrier system, C r C for rotating matrix around Y axis p For a rotation matrix about the Z-axis, C y For a rotation matrix about the X-axis, γ is the rotation angle about the Y-axis, p is the rotation angle about the X-axis, and Y is the rotation angle about the Z-axis.
4. The pedestrian indoor navigation method based on multi-source information fusion of claim 1, wherein the step of gait detection comprises:
calculating the overall acceleration:
wherein a is x ，a y ，a z For the collected triaxial acceleration value, the influence of the sensor posture is reduced by calculating the overall acceleration a;
according to the overall acceleration, a potential peak value is obtained in the sliding window, and primary judgment is carried out by utilizing a preset acceleration threshold value;
calculating the time difference between the potential wave crest and the previous wave crest, and performing secondary judgment by using a preset walking one-step time threshold range;
and comparing the potential peak with the front and rear neighborhood acceleration, judging for three times to remove the pseudo peak, if the potential peak point is the maximum value, recording one step by the algorithm, otherwise, not performing step counting processing.
5. The pedestrian indoor navigation method based on multi-source information fusion of claim 1, wherein the step size estimation is:
where SL is the step size, a max And a min The maximum and minimum of the longitudinal acceleration of the pedestrian, respectively.
6. The pedestrian indoor navigation method based on multi-source information fusion of claim 1, wherein the extended kalman filtering algorithm comprises:
X k+1 ＝f(X k )+w k
wherein X is k+1 X is the state variable of the (k+1) th step k Is the state variable of the kth step, w k Is process noise; f () is a nonlinear state function in extended kalman filtering;
the method for realizing the linearization of the system matrix comprises the following steps:
wherein Z is k To observe the variable, v k H () is a measurement function in extended kalman filtering for observing noise;
the method for realizing the linearization of the observation matrix comprises the following steps:
wherein H is k Is a jacobian matrix of h (X) to X bias,and (5) estimating the state of the kth step.
7. The pedestrian indoor navigation method based on multi-source information fusion of claim 1, wherein the step of expanding the state vector update in the kalman filter comprises:
one-step state prediction update:
in the method, in the process of the invention,is the optimal value of the last state, +.>A one-step predicted value for the current state;
one-step predictive estimation error covariance matrix update:
wherein P is k+1|k Is thatCorresponding covariance one-step predictor, phi k+1|k For state transition matrix, P k|k Is->Corresponding covariance,/>Is phi k+1|k Transpose of Q k A process noise covariance matrix;
calculating an extended Kalman filtering gain:
wherein K is k+1 To expand the Kalman filter gain matrix, H k+1 In order to observe the matrix,for transpose of the observation matrix, R k+1 Measuring a noise covariance matrix;
calculating information from the observation vectors and updating the state estimate:
in the method, in the process of the invention,z is the optimal estimated value of the current state k+1 For the current observation +.>The current observation predicted value;
updating the estimated error covariance:
P k+1 ＝[I-K k+1 H k+1 ]P k+1|k
wherein P is k+1 Is thatCorresponding covariance, I is the identity matrix.
8. The pedestrian indoor navigation method based on multi-source information fusion according to claim 1, wherein the further correcting and updating of the pedestrian position information according to the map information and the particle filter algorithm comprises:
the state transfer equation and the observation equation for particle filtering are as follows:
x(t)＝f(x(t-1),u(t),w(t))
y(t)＝f(x(t),e(t))
wherein x (t) is a state at time t, u (t) is a control quantity, w (t) and e (t) are state noise and observation noise respectively, and particle filtering filters the state at time t x (t) from observation y (t) and the state at the last time x (t-1), u (t) and w (t);
the updating method in particle filtering mainly comprises the following steps:
a1, initializing: initial value x of observed value i (k) As an initial value of the probability density function x (0), byFor->Sampling N times, initial w i (k) Set to->
A2, one-step prediction: for each particle x i (k) By converting the formula p (x (k+1) |x i (k) Obtaining a new particle;
a3, importance sampling: for any particle x i (k+1) solving for their weights w i (k+1)＝p(z(k+1)|x i (k+1))；
A4, normalization: and (3) carrying out normalization processing on the weight values:
a5, resampling: resampling the particles according to the weight, wherein the probability of repeating the particles with the weight is high, and the probability of repeating the particles with the weight is low;
a6, updating particles and simultaneously adding map information:
order theIs->
Because of the limit value of the indoor walking area, map matching is carried out according to map information, and when the particles predicted in one step cross the passable area, the weight value of the particles is given as 0:
and A7, calculating by adopting a basic particle filtering algorithm, and completing the indoor positioning and navigation of the pedestrians according to the calculation result.
9. A pedestrian indoor navigation device based on multi-source information fusion, comprising:
at least one of a processor;
at least one memory for storing at least one program;
the at least one program, when executed by the at least one processor, causes the at least one processor to implement the method of any one of claims 1-8.
10. A computer readable storage medium, in which a processor executable program is stored, characterized in that the processor executable program is for performing the method according to any of claims 1-8 when being executed by a processor.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310210607.1A CN116448111A (en) | 2023-03-06 | 2023-03-06 | Pedestrian indoor navigation method, device and medium based on multi-source information fusion |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310210607.1A CN116448111A (en) | 2023-03-06 | 2023-03-06 | Pedestrian indoor navigation method, device and medium based on multi-source information fusion |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116448111A true CN116448111A (en) | 2023-07-18 |
Family
ID=87129134
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310210607.1A Pending CN116448111A (en) | 2023-03-06 | 2023-03-06 | Pedestrian indoor navigation method, device and medium based on multi-source information fusion |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116448111A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118408552A (en) * | 2024-07-01 | 2024-07-30 | 山东科技大学 | Unmanned ship multi-sensor positioning method based on double-threshold event trigger mechanism |
| CN118444647A (en) * | 2024-07-08 | 2024-08-06 | 微晶数实(山东)装备科技有限公司 | Intelligent device control method, system and medium based on multi-source information fusion |
-
2023
- 2023-03-06 CN CN202310210607.1A patent/CN116448111A/en active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118408552A (en) * | 2024-07-01 | 2024-07-30 | 山东科技大学 | Unmanned ship multi-sensor positioning method based on double-threshold event trigger mechanism |
| CN118444647A (en) * | 2024-07-08 | 2024-08-06 | 微晶数实(山东)装备科技有限公司 | Intelligent device control method, system and medium based on multi-source information fusion |
| CN118444647B (en) * | 2024-07-08 | 2024-09-27 | 微晶数实(山东)装备科技有限公司 | Intelligent device control method, system and medium based on multi-source information fusion |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112639502B (en) | Robot pose estimation | |
| CN107635204B (en) | Indoor fusion positioning method and device assisted by exercise behaviors and storage medium | |
| CN110118560B (en) | Indoor positioning method based on LSTM and multi-sensor fusion | |
| CN111402339B (en) | Real-time positioning method, device, system and storage medium | |
| CN110007670B (en) | Mobile robot positioning and mapping method | |
| CN106714110B (en) | Wi-Fi position fingerprint map automatic construction method and system | |
| JP5219467B2 (en) | Method, apparatus, and medium for posture estimation of mobile robot based on particle filter | |
| CN103471589B (en) | The method of the identification of a kind of indoor pedestrian's walking mode and trajectory track | |
| CN116448111A (en) | Pedestrian indoor navigation method, device and medium based on multi-source information fusion | |
| CN112965063B (en) | Robot mapping and positioning method | |
| CN107255795A (en) | Localization Approach for Indoor Mobile and device based on EKF/EFIR mixed filterings | |
| KR101985344B1 (en) | Sliding windows based structure-less localization method using inertial and single optical sensor, recording medium and device for performing the method | |
| CN109141413B (en) | EFIR filtering algorithm and system with data missing UWB pedestrian positioning | |
| CN110501010A (en) | Determine position of the mobile device in geographic area | |
| CN112415558B (en) | Processing method of travel track and related equipment | |
| CN110388926B (en) | Indoor positioning method based on mobile phone geomagnetism and scene image | |
| Yuanfeng et al. | Flexible indoor localization and tracking system based on mobile phone | |
| CN115143954A (en) | Unmanned vehicle navigation method based on multi-source information fusion | |
| CN112797985A (en) | Indoor positioning method and indoor positioning system based on weighted extended Kalman filtering | |
| CN115164887A (en) | Pedestrian navigation positioning method and device based on laser radar and inertia combination | |
| CN111707294B (en) | Pedestrian navigation zero-speed interval detection method and device based on optimal interval estimation | |
| CN110989341B (en) | Constraint auxiliary particle filtering method and target tracking method | |
| CN111399021A (en) | Navigation positioning method | |
| CN114565670A (en) | Pose optimization method and device | |
| CN114608585A (en) | Method and device for synchronous positioning and mapping of mobile robot |
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 |