CN107246872B  Singleparticle filtering navigation device and method based on MEMS sensor and VLC positioning fusion  Google Patents
Singleparticle filtering navigation device and method based on MEMS sensor and VLC positioning fusion Download PDFInfo
 Publication number
 CN107246872B CN107246872B CN201710507321.4A CN201710507321A CN107246872B CN 107246872 B CN107246872 B CN 107246872B CN 201710507321 A CN201710507321 A CN 201710507321A CN 107246872 B CN107246872 B CN 107246872B
 Authority
 CN
 China
 Prior art keywords
 information
 positioning
 vlc
 module
 pdr
 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
 239000002245 particle Substances 0.000 title claims abstract description 82
 238000000034 method Methods 0.000 title claims abstract description 27
 238000001914 filtration Methods 0.000 title claims abstract description 22
 230000004927 fusion Effects 0.000 title claims abstract description 13
 230000003287 optical effect Effects 0.000 claims abstract description 12
 239000013598 vector Substances 0.000 claims description 59
 238000005259 measurement Methods 0.000 claims description 28
 238000012952 Resampling Methods 0.000 claims description 13
 239000011159 matrix material Substances 0.000 claims description 12
 230000008569 process Effects 0.000 claims description 12
 230000001133 acceleration Effects 0.000 claims description 10
 238000012545 processing Methods 0.000 claims description 5
 230000018199 S phase Effects 0.000 claims description 3
 230000005484 gravity Effects 0.000 claims description 3
 238000012546 transfer Methods 0.000 claims description 3
 230000009466 transformation Effects 0.000 claims description 3
 230000008859 change Effects 0.000 description 3
 238000005516 engineering process Methods 0.000 description 3
 230000010354 integration Effects 0.000 description 3
 238000004891 communication Methods 0.000 description 2
 230000007547 defect Effects 0.000 description 2
 238000013461 design Methods 0.000 description 2
 230000000694 effects Effects 0.000 description 2
 238000004088 simulation Methods 0.000 description 2
 238000012935 Averaging Methods 0.000 description 1
 230000000903 blocking effect Effects 0.000 description 1
 230000015556 catabolic process Effects 0.000 description 1
 230000003247 decreasing effect Effects 0.000 description 1
 238000006731 degradation reaction Methods 0.000 description 1
 238000001514 detection method Methods 0.000 description 1
 238000011161 development Methods 0.000 description 1
 238000010586 diagram Methods 0.000 description 1
 230000007613 environmental effect Effects 0.000 description 1
 230000004044 response Effects 0.000 description 1
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
Landscapes
 Engineering & Computer Science (AREA)
 Radar, Positioning & Navigation (AREA)
 Remote Sensing (AREA)
 Automation & Control Theory (AREA)
 Physics & Mathematics (AREA)
 General Physics & Mathematics (AREA)
 Navigation (AREA)
Abstract
The invention discloses a kind of singleparticles based on MEMS sensor and VLC positioning fusion to filter navigation device and method, including MEMS sensor, INS module, VLC locating module, PDR locating module and survey appearance position singleparticle filter module；System equation of the error equation as fused filtering device based on INS inertial navigation mechanization in the present invention, is updated for the particle to population.Observational equation includes that VLC positioning information update, PDR positioning information update and magnetometer observed quantity update, for calculating the weight of each particle.Posture information is exported to VLC locating module and PDR locating module.The present invention surveys the posture information that appearance accurately estimates VLC receiver using fusion in VLC positioning field for the first time, solve the problems, such as that VLC positioning is easy to be influenced by receiver posture and positioned when optical signal is blocked discontinuous, and eliminates the influence that posture positions VLC.
Description
Technical Field
The invention relates to an intelligent positioning device and method, in particular to a single particle filtering navigation device and method based on MEMS sensor and VLC positioning fusion.
Background
With the development and application of indoor positioning technology, indoor positioning technology based on visible light communication is rapidly emerging and is gaining wide attention. Under the environment of sufficient indoor light sources, optical signals modulated by a multiplexing protocol are obtained through detection of devices such as an optical sensor, and different light source signal data can be separated through a signal demodulation technology, so that the distance or angle information of a positioning target relative to each light source can be calculated by combining environmental parameters, and finally target positioning can be completed through a positioning algorithm such as trilateral positioning.
However, the VLC positioning result is greatly affected because the posture of the target receiving device may shake as the target moves. On the other hand, the light signal is easily blocked in the actual scene, which will result in discontinuous positioning. In response to the former problem, current solutions are primarily colocated through multiple sensor combinations. In view of the latter problem, the mainstream solution is to estimate the target position by kalman filtering or particle filtering. However, these solutions have some problems: 1) compared with singlesensor positioning, the multisensor combined positioning algorithm is complex and has higher cost; 2) the filter fusion proposed in the current positioning scheme is based on the premise that the detector posture is stable, and the stability is poor in the actual scene. 3) In a scene where signal occlusion frequently occurs, the target position inferred by the VLC data plus filter positioning system alone still has a large deviation from the actual position.
In addition, in inertial navigation positioning, Kalman filtering is generally adopted for noise processing and positioning, the Kalman filtering has a good effect on a linear system, and in an actual environment, many systems are nonlinear. The extended kalman filter is a new form of kalman filter, which converts a nonlinear system into a linear system by ignoring components of more than two orders after taylor expansion. The particle filtering algorithm can process a completely nonlinear system, and the effect is better than that of Kalman filtering.
Disclosure of Invention
The purpose of the invention is as follows: in order to solve the defects of the prior art, the attitude measurement positioning single particle filtering navigation device and method based on the integration of the MEMS sensor and the VLC positioning can eliminate the influence of the attitude on the VLC positioning and can make up the determination of the discontinuity and the unsmooth VLC positioning result.
The technical scheme is as follows: the single particle filtering navigation device based on the integration of the MEMS sensor and VLC positioning comprises an MEMS sensor, an INS module, a VLC positioning module, a PDR positioning module and an attitude measurement positioning single particle filter module; the MEMS sensor comprises an accelerometer, a gyroscope and a magnetometer;
the input of the attitude measurement positioning single particle filter module comprises the following steps: acceleration information of the receiver in the XYZ direction measured by the accelerometer and angular velocity information of the receiver in the XYZ direction measured by the gyroscope are transmitted to the INS module, and the INS module arranges the acceleration information and the angular velocity information to obtain INS position information, velocity information and posture information of the receiver; the angle information of the receiver relative to the east, south, west and north directions measured by the magnetometer; acceleration information of the receiver measured by the accelerometer in the XYZ direction is used for carrying out position estimation on PDR positioning information obtained by the PDR positioning module and position information obtained by weighted average of VLC positioning information output by the VLC positioning module;
the attitude measurement and positioning single particle filter module outputs position information, speed information and attitude information of the receiver at the current time to the INS mechanical arrangement module, outputs the attitude information of the receiver at the current time to the PDR positioning module or the VLC positioning module, outputs a noise compensation signal to the accelerometer and the gyroscope, and outputs the positioning information of the receiver at the current time.
Further, the weight of the PDR positioning information and the VLC positioning information is set by whether the light signal is blocked in the VLC positioning module, and if the light signal is blocked, the system sets the weight of the VLC position information to 0, and only the PDR position information is used.
A navigation method based on the navigation device, comprising the steps of:
(1) establishing a state vector of SPF;
(2) establishing a system model of SPF;
(3) establishing an observation equation of SPF;
(4) and SPF filtering and outputting positioning information.
Further, the state vector of SPF in step (1) is:
x＝[δr^{n} δv^{n} ψ b_{g} b_{a}]^{T}
wherein, δ r^{n}、δv^{n}、ψ、b_{g}And b_{a}Respectively, a position error vector, a velocity error vector, an attitude error vector, a gyroscope bias vector, and an accelerometer bias vector for the receiver.
Further, the step (2) comprises:
(21) inputting receiver data information acquired by an accelerometer and a gyroscope into an INS module for mechanical arrangement algorithm processing to obtain current position information, speed information and attitude information of the receiver and inputting the current position information, the speed information and the attitude information into an SPF (Sphase filter); and (3) carrying out coordinate transformation on the attitude matrix, wherein a coordinate transfer equation is as follows:
wherein,is r^{n}The first derivative of (a) is,is a navigationA position vector in the coordinate system, and,denotes latitude, λ denotes longitude and h denotes altitude;is v^{n}First derivative of v^{n}Is a threedimensional velocity vector; g^{n}Is the gravity vector in the navigation coordinate system;indicating a position increment;represents a speed increment; f. of^{b}Is a specific force vector in the carrier coordinate system; d^{1}Is a aboutAnd a 3 × 3 matrix of h;is thatThe first derivative of (a) is,the direction cosine matrix from a carrier coordinate system to a navigation coordinate system is planned and predicted by an INS machine;andrespectively angular velocity vector Anda skewsymmetric matrix of (a); whileAndrepresenting the angular velocity of earth selftransmission and the rotation angular velocity of a navigation coordinate system relative to a geocentric geostationary coordinate system;andrepresenting the rotation angular velocity of the carrier coordinate system relative to the inertial coordinate and the rotation angular velocity of the navigation coordinate system relative to the inertial coordinate;
(22) the system model of SPF isThe specific expansion formula is as follows:
wherein the delta sign represents the error, i.e. the difference between the true value and the system nominal value;andrespectively represent δ r^{n}、δv^{n}、ψ、b_{g}And b_{a}First derivative of f^{n}Is a specific force vector projected to the navigational coordinate system; w is a_{g}And w_{a}Is sensor noise; tau is_{bg}And τ_{ba}Representing the correlation time of inertial navigation noise; w is a_{bg}And w_{ba}Is the driving noise, and the symbol "x" represents cross multiplication.
Further, the step (3) comprises:
(31) position observation equation of PDR or VLC output
When the situation that the VLC cannot output effective positioning information due to the fact that the optical signal blocks occurs, the PDR position information is used as the input quantity of the attitude measurement positioning single particle filter module; the observation equation established according to the PDR location information is:
whereinAndmechanically arranging the calculated latitude and longitude for the INS module;andlatitude and longitude from PDR, respectively;and n_{λ}Is the measurement noise;
otherwise, the attitude measurement positioning single particle filter module receives VLC position information and ignores PDR position information; the observation equation established according to the VLC output information is as follows:
wherein,andrespectively mechanically arranging INS modulesThe position vector and the position vector obtained by the VLC positioning module; delta r^{n}Is a position error vector; n is_{1}Is the measurement noise;
(32) magnetometer observation equation
The filter updates the attitude directly through magnetometer readings, and the magnetometer observation equation is as follows:
wherein, is the magnetometer reading vector, m^{n}Is the calibrated LMF vector, n_{3}Is noise.
Further, the step (4) comprises:
(41) will input quantityAnd z_{k}Input SPF
Wherein,is the particle at the time of k1,represents the state of the ith particle at the time k1;represents the weight, z, of the ith particle at time k1_{k}Is the observed value at the time k; particle state quantity x ═ δ r^{n} δv^{n} ψ b_{g} b_{a}]^{T}；
(42) Filtering process
(a) For the state of the ith particle at each time k1Generating the state of the ith particle at the k moment through a system equation model (2)
(b) For each new particleCalculating a weight for each particle using a PDR or VLC observation equation and a magnetometer observation equation
Wherein,representing observed quantity z at time k_{k}For particlesThe conditional probability of (a);
(c) normalized weight
Wherein N is_{S}The number of particles is shown.
(d) Resampling process
(43) Filtered output
Outputting particles at time kAnd feeding back position information, speed information and attitude information in the particle state information at the moment k to the INS module, outputting the attitude information of the VLC receiver to the VLC positioning module, outputting the attitude information of the PDR receiver to the PDR positioning module to correct the influence of the attitude, outputting a gyro deviation vector to the gyroscope for noise compensation, and outputting receiver positioning information.
Has the advantages that: compared with the prior art, the invention has the following advantages: 1) the method comprises the steps that a single particle filter for measuring the attitude is used for accurately estimating attitude information of a VLC receiver for the first time in the VLC positioning field by using fusion attitude measurement, and the influence of the attitude on VLC positioning is eliminated; 2) the design can make up the defects of discontinuous and unsmooth VLC positioning results; 3) this design may provide positioning results using MEMS sensor information in the event that VLC signals are obscured.
Drawings
FIG. 1 is a schematic structural diagram of a single particle filter navigation device for attitude determination positioning;
FIG. 2 is a CDF plot of simulated positioning results for a pitch angle of 0;
FIG. 3 is a CDF plot of simulated positioning results for a 5 ° pitch angle;
FIG. 4 is a CDF plot of simulated positioning results at 8 ° pitch;
fig. 5 shows the results of the analysis of the positioning error when the receiving device is laid flat, tilted by 5 ° and tilted by 8 °.
Detailed Description
The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.
As shown in fig. 1, the attitude measurement positioning single particle filter navigation device based on the integration of MEMS (Microelectro mechanical Systems) sensors and VLC positioning includes: the System comprises a MEMS sensor, an Inertial Navigation System (INS) module, a Visible Light Communication (VLC) positioning module, a Pedestrian Dead Reckoning (PDR) positioning module and a Single Particle Filter (SPF) module, wherein the MEMS sensor comprises an accelerometer, a gyroscope and a magnetometer.
One path of an acceleration signal of the accelerometer measurement receiver in the XYZ direction is transmitted to the PDR positioning module for position estimation, the other path of the acceleration signal is transmitted to the INS mechanical arrangement module, and an angular velocity signal of the gyroscope measurement receiver in the XYZ direction is transmitted to the INS mechanical arrangement module. And the INS mechanical arrangement module is used for mechanically arranging the acceleration signals and the angular velocity signals through a mechanical arrangement algorithm to obtain INS position information, velocity information and posture information. And the PDR positioning module carries out position estimation on the acceleration signal to obtain PDR position information. The magnetometer measures the angular information of the receiver relative to the east, south, west and north directions, and the VLC locating module measures the position information of the receiver.
In addition, although the VLC positioning module and the PDR positioning module perform positioning at the same time, the positioning results are not equivalently input into the pose measurement positioning single particle filter module. Generally, the single particle filter module for measuring attitude and positioning mainly receives the position information of the VLC positioning module, and ignores the position information of the PDR positioning module. The position information of the PDR module is used as the main input only when the VLC positioning module cannot output valid positioning information, such as light signal blocking.
Namely, the position information of the PDR positioning module and the position information of the VLC positioning module after weighted averaging is used as unique position information to be input into the attitude measurement positioning single particle filter module, when the light signal in the VLC positioning module is shielded, the system sets the weight of the position information of the VLC positioning module to be 0, and only the position information of the PDR module is adopted.
PDR position information, VLC position information, angle information measured by the magnetometer, INS position information, speed information and attitude information are input into the attitude measurement positioning single particle filter module as input quantities, the position information, the speed information and the attitude information of the receiver at the current moment are output to the INS mechanical arrangement module through filter processing, the attitude information of the receiver at the current moment is output to the PDR module or the VLC positioning module, a gyroscope deviation vector is output to the gyroscope, an accelerometer deviation vector is output to the accelerometer, the positioning information of the receiver at the current moment is output, and meanwhile, the state quantity at the next moment is updated.
The single particle Filter (SPF) for measuring posture, positioning and positioning is mainly used for modeling a state vector, a system equation and a state equation. The SPF is mainly used to estimate attitude information (i.e., roll angle, pitch angle, and azimuth angle) and position information (i.e., longitude, latitude, and altitude) of the receiver.
A navigation method based on the navigation device comprises the following steps:
(1) establishing a state vector of SPF
The state vector of SPF is defined as:
x＝[δr^{n} δv^{n} ψ b_{g} b_{a}]^{T} (1)
wherein, δ r^{n}、δv^{n}、ψ、b_{g}And b_{a}Respectively, a position error vector, a velocity error vector, an attitude error vector, a gyroscope bias vector, and an accelerometer bias vector for the receiver.
(2) System model for establishing SPF
(21) Inputting receiver data information acquired by an accelerometer and a gyroscope into an INS mechanical arrangement module to perform mechanical arrangement algorithm processing to obtain current position information, speed information and attitude information of the receiver, and inputting the current position information, the speed information and the attitude information into an SPF (Sphase filter); and (3) carrying out coordinate transformation on the attitude matrix, wherein a coordinate transfer equation is as follows:
wherein,is r^{n}The first derivative of (a) is,is a position vector in the navigation coordinate system,denotes latitude, λ denotes longitude and h denotes altitude;is v^{n}First derivative of v^{n}Is a threedimensional velocity vector; g^{n}Is the gravity vector in the navigation coordinate system;indicating a position increment;represents a speed increment; f. of^{b}Is a specific force vector in the carrier coordinate system; d^{1}Is a aboutAnd a 3 × 3 matrix of h;is thatThe first derivative of (a) is,the direction cosine matrix from a carrier coordinate system to a navigation coordinate system is planned and predicted by an INS machine;andrespectively angular velocity vector Anda skewsymmetric matrix of (a); whileAndrepresenting the angular velocity of earth selftransmission and the rotation angular velocity of a navigation coordinate system relative to a geocentric geostationary coordinate system;andrepresenting the angular velocity of rotation of the carrier coordinate system relative to the inertial coordinates and the angular velocity of rotation of the navigation coordinate system relative to the inertial coordinates.
(22) The system model of SPF isThe specific expansion formula is as follows:
wherein the delta sign represents the error, i.e. the difference between the true value and the system nominal value;andrespectively represent δ r^{n}、δv^{n}、ψ、b_{g}And b_{a}First derivative of f^{n}Is a specific force vector projected to the navigational coordinate system; w is a_{g}And w_{a}Is sensor noise; tau is_{bg}And τ_{ba}Representing the correlation time of inertial navigation noise; w is a_{bg}And w_{ba}Is the driving noise, and the symbol "x" represents cross multiplication.
(3) Establishing an observation equation for SPF
The observation equation of SPF consists of two parts: 1) PDR or VLC output position observation equation 2) magnetometer observation equation. It should be noted that different observation equations should be employed depending on whether PDR position information or VLC is employed as input.
(31) Position observation equation of PDR or VLC output
When the situation that the VLC cannot output effective positioning information due to the fact that the optical signal blocks occurs, the position information of the PDR is used as the main input quantity of the attitude measurement positioning single particle filter module; the observation equation established from the PDR position is:
whereinAndcompiling the calculated latitude and longitude for the INS machine;andlatitude and longitude from PDR;and n_{λ}Is the measurement noise.
Otherwise, the attitude measurement positioning single particle filter module mainly receives the position information of the VLC and ignores the positioning information of the PDR; the observation equation established according to the VLC output information is as follows:
wherein,andposition vectors from INS mechanical programming and VLC, respectively; delta r^{n}Is a position error vector; n is_{1}Is the measurement noise.
The weight of the PDR positioning information and the weight of the VLC positioning information are set by judging whether the light signal in the VLC positioning module is shielded, if the light signal is shielded, the weight of the VLC position information is set to be 0 by the system, and only the PDR position information is adopted.
(32) Magnetometer observation equation
The filter updates the attitude directly through magnetometer readings, and the magnetometer observation equation is as follows:
wherein, is the magnetometer reading vector, m^{n}Is the calibrated LMF vector, n_{3}Is noise.
(4) SPF filtering implementation process
(41) Will input quantityAnd z_{k}Input SPF
Wherein,is the particle at the time of k1,represents the state of the ith particle at the time k1;represents the weight, z, of the ith particle at time k1_{k}Is the observed value at the time k; particle state quantity x ═ δ r^{n} δv^{n} ψ b_{g} b_{a}]^{T}；
The initial input of SPF is n particles x with same weight omega, and the value of the number n of the particles can be freely set.
(42) Filtering process
(a) For the state of the ith particle at each time k1By system equation model (2)Generating the state of the ith particle at time k
(b) For each new particleCalculating the weight of each particle using the observation equations (46)
Wherein,representing observed quantity z at time k_{k}For particlesThe conditional probability of (a);
(c) normalized weight
(d) Resampling process
In the filtering process, the weights of some particles with similar state quantities are gradually increased and tend to be the same, and the weights of most of the rest particles are gradually decreased, so that the diversity of the particles is lost, and the state estimation generates a large deviation, which is the problem of particle degradation. To solve this problem, a resampling process is generally introduced in the particle filtering. The resampling process comprises important resampling, residual resampling, layered resampling, optimized combined resampling and the like, and any one of the important resampling, the residual resampling, the layered resampling, the optimized combined resampling and the like can be selected. For example, the process duplicates the particles with large weights into a corresponding number of duplicates according to the weights of the particles, and eliminates the particles with small weights, but the total number remains unchanged.
(43) Filtered output
Outputting particles at time kAnd feeding back position information, speed information and posture information in the particle state information at the moment k to the INS module, outputting the posture information of the VLC receiver to the VLC positioning module, and outputting the posture information of the PDR receiver to the PDR positioning module so as to correct the influence of the posture. And outputting the gyro deviation vector to feed back to the gyroscope for noise compensation. The receiver positioning information (position information and velocity information) is output.
The fusion filter can accurately estimate the attitude information of the VLC receiver by using the fusion attitude measurement in the VLC positioning field for the first time, and eliminate the influence of the attitude on VLC positioning. And taking an error equation based on INS inertial navigation mechanical arrangement as a system equation of the fusion filter, and updating the particles of the particle swarm. And the observation equation comprises VLC positioning information updating, PDR positioning information updating and magnetometer observed quantity updating and is used for calculating the weight of each particle. The fusion filter outputs the posture of the VLC receiving device to the VLC positioning module, and outputs the posture of the PDR device to the PDR positioning module to correct the influence of the posture.
The technical principle is as follows:
in the VLC positioning module, the positioning algorithm generally adopts a trilateral positioning algorithm. The principle is that a plurality of lamps at the top end of a room are used as an emission source, generally, at least three LED lamps are used, the distance from a target point to each lamp is estimated by replacing an optical signal propagation model with the optical signal intensity measured by a receiver in real time, and finally, the positioning position is estimated by a simultaneous equation. However, when the optical signal is blocked, the detector cannot acquire all signals, the solved position can seriously exceed the error range, and the condition can be eliminated through a threshold value, so that the system does not output position information under the condition, and the positioning result is discontinuous and unsmooth. In addition, considering that random noise such as shot noise and thermal noise often exists in a VLC system, and a certain error often exists in estimation of a distance, so that a motion trajectory formed by an estimated position has characteristics such as a steep change, which often does not conform to motion characteristics of an actual target. Since the priori knowledge should indicate that the positioning track is smooth when the target tracks, the state of the target at the current moment is related to the state at the previous moment, and the filtering method can take the priori value into account, so that the positioning track is smoother.
Currently, the general VLC positioning often assumes that the device is in a flat posture, thereby simplifying the positioning algorithm. However, in practical situations the device will be jittered and tilted. Random tilting of the device changes the angle of incidence of the optical signal, resulting in a change in the measured intensity of the optical signal. If the attitude of the device cannot be accurately estimated, the change of the intensity of the optical signal in the part becomes a part of the system error, so that the positioning error becomes large. In order to illustrate the influence degree of the attitude error on the VLC positioning precision, the VLC positioning precision under the conditions of different pitch angles is simulated through comparison. The simulation experiment simulates the situation that the receiver is inclined but the positioning algorithm is not corrected, two different inclination situations are simulated for all the receivers, the CDF curves of the positioning results are respectively shown in fig. 3 and fig. 4, and fig. 2 shows the positioning result that the equipment is not inclined. The experimental results show that the maximum positioning error is about 0.13m and 0.21m for 5 ° and 8 ° receiver pitch angles, respectively. Comparing the positioning results when the receiver is tilted and laid flat, as shown in fig. 5, it can be seen that when the receiver is tilted, the positioning error increases significantly if the receiver attitude is not corrected. The simulation result proves that the accurate estimation of the attitude of the receiver has important significance for improving the performance of the visible light positioning system. Therefore, the MEMS attitude estimation module is introduced, accurate attitude information is transmitted to the VLC positioning module for attitude calibration, and the influence of the attitude on the VLC positioning result can be obviously reduced.
Because VLC positioning can not be carried out under the condition that the light signal is blocked, a PDR positioning module is introduced for positioning. The PDR positioning can completely utilize MEMS sensor signals to calculate step length and direction, and the current position of a traveler is estimated by utilizing the position of the pedestrian at the previous moment. And obtaining the motion trail of the pedestrian by repeated iteration. Therefore, when signal shielding occurs in VLC positioning, the position result cannot be output, and the fusion system can adopt the positioning result of the PDR positioning module to make up.
Claims (7)
1. Single particle filtering navigation head based on MEMS sensor and VLC location fusion, its characterized in that: the system comprises an MEMS sensor, an INS module, a VLC positioning module, a PDR positioning module and an attitude measurement positioning single particle filter module; the MEMS sensor comprises an accelerometer, a gyroscope and a magnetometer;
the input of the attitude measurement positioning single particle filter module comprises the following steps: acceleration information of the receiver in the XYZ direction measured by the accelerometer and angular velocity information of the receiver in the XYZ direction measured by the gyroscope are transmitted to the INS module, and the INS module arranges the acceleration information and the angular velocity information to obtain INS position information, velocity information and posture information of the receiver; the angle information of the receiver relative to the east, south, west and north directions measured by the magnetometer; acceleration information of the receiver measured by the accelerometer in the XYZ direction is used for carrying out position estimation on PDR positioning information obtained by the PDR positioning module and position information obtained by weighted average of VLC positioning information output by the VLC positioning module;
the attitude measurement and positioning single particle filter module outputs position information, speed information and attitude information of the receiver at the current time to the INS mechanical arrangement module, outputs the attitude information of the receiver at the current time to the PDR positioning module or the VLC positioning module, outputs a noise compensation signal to the accelerometer and the gyroscope, and outputs the positioning information of the receiver at the current time.
2. The navigation device of claim 1, wherein: the weight of the PDR positioning information and the weight of the VLC positioning information are set by judging whether the light signal in the VLC positioning module is shielded, if the light signal is shielded, the weight of the VLC position information is set to be 0 by the system, and only the PDR position information is adopted.
3. A navigation method based on the navigation device of claim 1 or 2, comprising the steps of:
(1) establishing a state vector of an attitude measurement positioning single particle filter SPF;
(2) establishing a system model of SPF;
(3) establishing an observation equation of SPF;
(4) and SPF filtering and outputting positioning information.
4. A navigation method according to claim 3, characterized in that: the state vector of SPF in the step (1) is:
x＝[δr^{n} δv^{n} ψ b_{g} b_{a}]^{T}
wherein, δ r^{n}、δv^{n}、ψ、b_{g}And b_{a}Respectively, a position error vector, a velocity error vector, an attitude error vector, a gyroscope bias vector, and an accelerometer bias vector for the receiver.
5. A navigation method according to claim 4, wherein the step (2) comprises:
(21) inputting receiver data information acquired by an accelerometer and a gyroscope into an INS module for mechanical arrangement algorithm processing to obtain current position information, speed information and attitude information of the receiver and inputting the current position information, the speed information and the attitude information into an SPF (Sphase filter); and (3) carrying out coordinate transformation on the attitude matrix, wherein a coordinate transfer equation is as follows:
wherein,is r^{n}The first derivative of (a) is,is a position vector in the navigation coordinate system,denotes latitude, λ denotes longitude and h denotes altitude;is v^{n}First derivative of v^{n}Is a threedimensional velocity vector; g^{n}Is a navigation coordinate systemThe gravity vector of (1);indicating a position increment;represents a speed increment; f. of^{b}Is a specific force vector in the carrier coordinate system; d^{1}Is a aboutAnd a 3 × 3 matrix of h;is thatThe first derivative of (a) is,the direction cosine matrix from a carrier coordinate system to a navigation coordinate system is planned and predicted by an INS machine;andrespectively angular velocity vector Anda skewsymmetric matrix of (a); whileAndrepresenting the angular velocity of earth selftransmission and the rotation angular velocity of a navigation coordinate system relative to a geocentric geostationary coordinate system;andrepresenting the rotation angular velocity of the carrier coordinate system relative to the inertial coordinate and the rotation angular velocity of the navigation coordinate system relative to the inertial coordinate;
(22) the system model of SPF isThe specific expansion formula is as follows:
wherein the delta sign represents the error, i.e. the difference between the true value and the system nominal value;andrespectively represent δ r^{n}、δv^{n}、ψ、b_{g}And b_{a}First derivative of f^{n}Is a specific force vector projected to the navigational coordinate system; w is a_{g}And w_{a}Is sensor noise; tau is_{bg}And τ_{ba}Representing the correlation time of inertial navigation noise; w is a_{bg}And w_{ba}Is the driving noise, and the symbol "x" represents cross multiplication.
6. A navigation method according to claim 5, wherein the step (3) comprises:
(31) position observation equation of PDR or VLC output
When the situation that the VLC cannot output effective positioning information due to the fact that the optical signal blocks occurs, the PDR position information is used as the input quantity of the attitude measurement positioning single particle filter module; the observation equation established according to the PDR location information is:
whereinAndmechanically arranging the calculated latitude and longitude for the INS module;andlatitude and longitude from PDR, respectively;and n_{λ}Is the measurement noise;
otherwise, the attitude measurement positioning single particle filter module receives VLC position information and ignores PDR position information; the observation equation established according to the VLC output information is as follows:
wherein,andare respectively INS modulesThe position vector obtained by the mechanical layout position vector and VLC positioning module; delta r^{n}Is a position error vector; n is_{1}Is the measurement noise;
(32) magnetometer observation equation
The filter updates the attitude directly through magnetometer readings, and the magnetometer observation equation is as follows:
wherein, is the magnetometer reading vector, m^{n}Is the calibrated LMF vector, n_{3}Is noise.
7. A navigation method according to claim 6, wherein said step (4) comprises:
(41) will input quantityAnd z_{k}Input SPF
Wherein,is the particle at the time of k1,represents the state of the ith particle at the time k1;represents the weight, z, of the ith particle at time k1_{k}Is the observed value at the time k; particle state quantity x ═ δ r^{n} δv^{n} ψ b_{g} b_{a}]^{T}；
(42) Filtering process
(a) For the state of the ith particle at each time k1Generating the state of the ith particle at the k moment through the SPF system model established in the step (2)
(b) For each new particleCalculating a weight for each particle using a PDR or VLC observation equation and a magnetometer observation equation
Wherein,representing observed quantity z at time k_{k}For particlesIs also referred to as a probability density function;
(c) normalized weight
Wherein N is_{S}The number of particles;
(d) resampling process
(43) Filtered output
Outputting particles at time kAnd the position information, the speed information and the posture in the particle state information at the time kThe attitude information is fed back to the INS module, the attitude information of the VLC receiver is output to the VLC positioning module, the attitude information of the PDR receiver is output to the PDR positioning module, the influence of the attitude is corrected, the gyro deviation vector is output to the gyroscope for noise compensation, and the receiver positioning information is output.
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN201710507321.4A CN107246872B (en)  20170628  20170628  Singleparticle filtering navigation device and method based on MEMS sensor and VLC positioning fusion 
Applications Claiming Priority (1)
Application Number  Priority Date  Filing Date  Title 

CN201710507321.4A CN107246872B (en)  20170628  20170628  Singleparticle filtering navigation device and method based on MEMS sensor and VLC positioning fusion 
Publications (2)
Publication Number  Publication Date 

CN107246872A CN107246872A (en)  20171013 
CN107246872B true CN107246872B (en)  20191011 
Family
ID=60014860
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN201710507321.4A Active CN107246872B (en)  20170628  20170628  Singleparticle filtering navigation device and method based on MEMS sensor and VLC positioning fusion 
Country Status (1)
Country  Link 

CN (1)  CN107246872B (en) 
Families Citing this family (5)
Publication number  Priority date  Publication date  Assignee  Title 

CN109997014B (en) *  20171103  20230818  北京嘀嘀无限科技发展有限公司  System and method for determining trajectory 
CN107869993A (en) *  20171107  20180403  西北工业大学  Small satellite attitude method of estimation based on adaptive iteration particle filter 
CN109579834B (en) *  20181224  20201204  北京全电智领科技有限公司  Positioning method and device based on space optical communication technology 
CN111198365A (en) *  20200116  20200526  东方红卫星移动通信有限公司  Indoor positioning method based on radio frequency signal 
CN111551181A (en) *  20200529  20200818  深圳市南科信息科技有限公司  Indoor positioning method based on dead reckoning of smart phone and LiFi identification 
Citations (5)
Publication number  Priority date  Publication date  Assignee  Title 

CN103968827A (en) *  20140409  20140806  北京信息科技大学  Wearable human body gait detection selflocalization method 
CN104215243A (en) *  20141013  20141217  北京大学工学院南京研究院  Passive autonomous indoor locating system based on Android system for medical application 
CN104215238A (en) *  20140821  20141217  北京空间飞行器总体设计部  Indoor positioning method of intelligent mobile phone 
CN104713554A (en) *  20150201  20150617  北京工业大学  Indoor positioning method based on MEMS insert device and android smart mobile phone fusion 
CN104819716A (en) *  20150421  20150805  北京工业大学  Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (microelectromechanical system) 

2017
 20170628 CN CN201710507321.4A patent/CN107246872B/en active Active
Patent Citations (5)
Publication number  Priority date  Publication date  Assignee  Title 

CN103968827A (en) *  20140409  20140806  北京信息科技大学  Wearable human body gait detection selflocalization method 
CN104215238A (en) *  20140821  20141217  北京空间飞行器总体设计部  Indoor positioning method of intelligent mobile phone 
CN104215243A (en) *  20141013  20141217  北京大学工学院南京研究院  Passive autonomous indoor locating system based on Android system for medical application 
CN104713554A (en) *  20150201  20150617  北京工业大学  Indoor positioning method based on MEMS insert device and android smart mobile phone fusion 
CN104819716A (en) *  20150421  20150805  北京工业大学  Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (microelectromechanical system) 
Also Published As
Publication number  Publication date 

CN107246872A (en)  20171013 
Similar Documents
Publication  Publication Date  Title 

CN107246872B (en)  Singleparticle filtering navigation device and method based on MEMS sensor and VLC positioning fusion  
CN107289932B (en)  Single deck taperecorder Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion  
CN107289933B (en)  Double card Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion  
CN107270898B (en)  Double particle filter navigation devices and method based on MEMS sensor and VLC positioning fusion  
US10216265B1 (en)  System and method for hybrid optical/inertial headtracking via numerically stable Kalman filter  
US10440678B2 (en)  Estimating locations of mobile devices in a wireless tracking system  
CN110487267B (en)  Unmanned aerial vehicle navigation system and method based on VIO & UWB loose combination  
KR101347838B1 (en)  Motion capture device and associated method  
CN110686671B (en)  Indoor 3D realtime positioning method and device based on multisensor information fusion  
US20160238395A1 (en)  Method for indoor and outdoor positioning and portable device implementing such a method  
Bezick et al.  Inertial navigation for guided missile systems  
KR20180062137A (en)  Method for position estimation of hybird motion capture system  
KR101576424B1 (en)  Automatic calibration method of magnetometer for indoor positioning  
CN107024206A (en)  A kind of integrated navigation system based on GGI/GPS/INS  
CN108627152A (en)  A kind of air navigation aid of the miniature drone based on Fusion  
CN110044377A (en)  A kind of IMU offline calibration method based on Vicon  
Goppert et al.  Invariant Kalman filter application to optical flow based visual odometry for UAVs  
CN109883416A (en)  A kind of localization method and device of the positioning of combination visible light communication and inertial navigation positioning  
KR102095135B1 (en)  Method of positioning indoor and apparatuses performing the same  
Zhang et al.  MagODO: Motion speed estimation for indoor robots based on dual magnetometers  
Schlaile et al.  Using natural features for vision based navigation of an indoorVTOL MAV  
Elbes et al.  Gyroscope drift correction based on TDoA technology in support of pedestrian dead reckoning  
CN113660724B (en)  Motion trajectory determination method and device, computer equipment and storage medium  
KR20200020200A (en)  Indoor 3D location estimating system and method using multiple sensors  
CN114894180A (en)  Multisource fusion navigation method and system based on relative navigation information 
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 