CN107246872B - Single-particle filtering navigation device and method based on MEMS sensor and VLC positioning fusion - Google Patents
Single-particle 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 single-particles 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 single-particle 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 co-located 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 single-sensor positioning, the multi-sensor 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 S-PF;
(2) establishing a system model of S-PF;
(3) establishing an observation equation of S-PF;
(4) and S-PF filtering and outputting positioning information.
Further, the state vector of S-PF in step (1) is:
x＝[δrn δvn ψ bg ba]T
wherein, δ rn、δvn、ψ、bgAnd baRespectively, 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 S-PF (S-phase filter); and (3) carrying out coordinate transformation on the attitude matrix, wherein a coordinate transfer equation is as follows:
wherein,is rnThe first derivative of (a) is,is a navigationA position vector in the coordinate system, and,denotes latitude, λ denotes longitude and h denotes altitude;is vnFirst derivative of vnIs a three-dimensional velocity vector; gnIs the gravity vector in the navigation coordinate system;indicating a position increment;represents a speed increment; f. ofbIs a specific force vector in the carrier coordinate system; d-1Is 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 skew-symmetric matrix of (a); whileAndrepresenting the angular velocity of earth self-transmission 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 S-PF 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 δ rn、δvn、ψ、bgAnd baFirst derivative of fnIs a specific force vector projected to the navigational coordinate system; w is agAnd waIs sensor noise; tau isbgAnd τbaRepresenting the correlation time of inertial navigation noise; w is abgAnd wbaIs 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 rnIs a position error vector; n is1Is 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, mnIs the calibrated LMF vector, n3Is noise.
Further, the step (4) comprises:
(41) will input quantityAnd zkInput S-PF
Wherein,is the particle at the time of k-1,represents the state of the ith particle at the time k-1;represents the weight, z, of the ith particle at time k-1kIs the observed value at the time k; particle state quantity x ═ δ rn δvn ψ bg ba]T；
(42) Filtering process
(a) For the state of the ith particle at each time k-1Generating 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 kkFor particlesThe conditional probability of (a);
(c) normalized weight
Wherein N isSThe 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 (Micro-electro 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 (S-PF) 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 (S-PF) for measuring posture, positioning and positioning is mainly used for modeling a state vector, a system equation and a state equation. The S-PF 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 S-PF
The state vector of S-PF is defined as:
x＝[δrn δvn ψ bg ba]T (1)
wherein, δ rn、δvn、ψ、bgAnd baRespectively, 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 S-PF
(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 S-PF (S-phase filter); and (3) carrying out coordinate transformation on the attitude matrix, wherein a coordinate transfer equation is as follows:
wherein,is rnThe first derivative of (a) is,is a position vector in the navigation coordinate system,denotes latitude, λ denotes longitude and h denotes altitude;is vnFirst derivative of vnIs a three-dimensional velocity vector; gnIs the gravity vector in the navigation coordinate system;indicating a position increment;represents a speed increment; f. ofbIs a specific force vector in the carrier coordinate system; d-1Is 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 skew-symmetric matrix of (a); whileAndrepresenting the angular velocity of earth self-transmission 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 S-PF 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 δ rn、δvn、ψ、bgAnd baFirst derivative of fnIs a specific force vector projected to the navigational coordinate system; w is agAnd waIs sensor noise; tau isbgAnd τbaRepresenting the correlation time of inertial navigation noise; w is abgAnd wbaIs the driving noise, and the symbol "x" represents cross multiplication.
(3) Establishing an observation equation for S-PF
The observation equation of S-PF 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 rnIs a position error vector; n is1Is 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, mnIs the calibrated LMF vector, n3Is noise.
(4) S-PF filtering implementation process
(41) Will input quantityAnd zkInput S-PF
Wherein,is the particle at the time of k-1,represents the state of the ith particle at the time k-1;represents the weight, z, of the ith particle at time k-1kIs the observed value at the time k; particle state quantity x ═ δ rn δvn ψ bg ba]T；
The initial input of S-PF 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 k-1By 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 (4-6)
Wherein,representing observed quantity z at time kkFor 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 S-PF;
(2) establishing a system model of S-PF;
(3) establishing an observation equation of S-PF;
(4) and S-PF filtering and outputting positioning information.
4. A navigation method according to claim 3, characterized in that: the state vector of S-PF in the step (1) is:
x＝[δrn δvn ψ bg ba]T
wherein, δ rn、δvn、ψ、bgAnd baRespectively, 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 S-PF (S-phase filter); and (3) carrying out coordinate transformation on the attitude matrix, wherein a coordinate transfer equation is as follows:
wherein,is rnThe first derivative of (a) is,is a position vector in the navigation coordinate system,denotes latitude, λ denotes longitude and h denotes altitude;is vnFirst derivative of vnIs a three-dimensional velocity vector; gnIs a navigation coordinate systemThe gravity vector of (1);indicating a position increment;represents a speed increment; f. ofbIs a specific force vector in the carrier coordinate system; d-1Is 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 skew-symmetric matrix of (a); whileAndrepresenting the angular velocity of earth self-transmission 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 S-PF 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 δ rn、δvn、ψ、bgAnd baFirst derivative of fnIs a specific force vector projected to the navigational coordinate system; w is agAnd waIs sensor noise; tau isbgAnd τbaRepresenting the correlation time of inertial navigation noise; w is abgAnd wbaIs 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 rnIs a position error vector; n is1Is 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, mnIs the calibrated LMF vector, n3Is noise.
7. A navigation method according to claim 6, wherein said step (4) comprises:
(41) will input quantityAnd zkInput S-PF
Wherein,is the particle at the time of k-1,represents the state of the ith particle at the time k-1;represents the weight, z, of the ith particle at time k-1kIs the observed value at the time k; particle state quantity x ═ δ rn δvn ψ bg ba]T；
(42) Filtering process
(a) For the state of the ith particle at each time k-1Generating the state of the ith particle at the k moment through the S-PF 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 kkFor particlesIs also referred to as a probability density function;
(c) normalized weight
Wherein N isSThe 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) | 2017-06-28 | 2017-06-28 | Single-particle 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) | 2017-06-28 | 2017-06-28 | Single-particle filtering navigation device and method based on MEMS sensor and VLC positioning fusion |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107246872A CN107246872A (en) | 2017-10-13 |
| CN107246872B true CN107246872B (en) | 2019-10-11 |
Family
ID=60014860
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710507321.4A Active CN107246872B (en) | 2017-06-28 | 2017-06-28 | Single-particle 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) * | 2017-11-03 | 2023-08-18 | 北京嘀嘀无限科技发展有限公司 | System and method for determining trajectory |
| CN107869993A (en) * | 2017-11-07 | 2018-04-03 | 西北工业大学 | Small satellite attitude method of estimation based on adaptive iteration particle filter |
| CN109579834B (en) * | 2018-12-24 | 2020-12-04 | 北京全电智领科技有限公司 | Positioning method and device based on space optical communication technology |
| CN111198365A (en) * | 2020-01-16 | 2020-05-26 | 东方红卫星移动通信有限公司 | Indoor positioning method based on radio frequency signal |
| CN111551181A (en) * | 2020-05-29 | 2020-08-18 | 深圳市南科信息科技有限公司 | 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) * | 2014-04-09 | 2014-08-06 | 北京信息科技大学 | Wearable human body gait detection self-localization method |
| CN104215243A (en) * | 2014-10-13 | 2014-12-17 | 北京大学工学院南京研究院 | Passive autonomous indoor locating system based on Android system for medical application |
| CN104215238A (en) * | 2014-08-21 | 2014-12-17 | 北京空间飞行器总体设计部 | Indoor positioning method of intelligent mobile phone |
| CN104713554A (en) * | 2015-02-01 | 2015-06-17 | 北京工业大学 | Indoor positioning method based on MEMS insert device and android smart mobile phone fusion |
| CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
-
2017
- 2017-06-28 CN CN201710507321.4A patent/CN107246872B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103968827A (en) * | 2014-04-09 | 2014-08-06 | 北京信息科技大学 | Wearable human body gait detection self-localization method |
| CN104215238A (en) * | 2014-08-21 | 2014-12-17 | 北京空间飞行器总体设计部 | Indoor positioning method of intelligent mobile phone |
| CN104215243A (en) * | 2014-10-13 | 2014-12-17 | 北京大学工学院南京研究院 | Passive autonomous indoor locating system based on Android system for medical application |
| CN104713554A (en) * | 2015-02-01 | 2015-06-17 | 北京工业大学 | Indoor positioning method based on MEMS insert device and android smart mobile phone fusion |
| CN104819716A (en) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | Indoor and outdoor personal navigation algorithm based on INS/GPS (inertial navigation system/global position system) integration of MEMS (micro-electromechanical system) |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107246872A (en) | 2017-10-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107246872B (en) | Single-particle filtering navigation device and method based on MEMS sensor and VLC positioning fusion | |
| CN107289932B (en) | Single deck tape-recorder 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 real-time positioning method and device based on multi-sensor 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 off-line 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. | Mag-ODO: Motion speed estimation for indoor robots based on dual magnetometers | |
| Schlaile et al. | Using natural features for vision based navigation of an indoor-VTOL 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) | Multi-source 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 |