CN106643713A - Zero-velocity correct walker trajectory estimation method and device for smooth and self-adaptive threshold value adjustment - Google Patents
Zero-velocity correct walker trajectory estimation method and device for smooth and self-adaptive threshold value adjustment Download PDFInfo
- Publication number
- CN106643713A CN106643713A CN201611196322.3A CN201611196322A CN106643713A CN 106643713 A CN106643713 A CN 106643713A CN 201611196322 A CN201611196322 A CN 201611196322A CN 106643713 A CN106643713 A CN 106643713A
- Authority
- CN
- China
- Prior art keywords
- trough
- pedestrian
- angular speed
- zero
- value
- 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.)
- Granted
Links
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
The invention discloses a zero-velocity correct walker trajectory estimation method and device for smooth and self-adaptive threshold value adjustment. According to the method and the device, the original data is compensated, so that reliable original data is obtained; the frequency of steps is extracted by carrying out lowpass filtering and second judgment on the compensated acceleration data; after that, the threshold value of an angular velocity energy average is adjusted by taking the frequency of steps as an input variable, so that the aim of self-adaptive threshold value adjustment is achieved. Compared with the existing universal self-adaptive threshold value adjustment method, the method provided by the invention can realize smooth threshold value adjustment so as to adapt to the actions with various frequencies of steps of walkers; therefore, the problems of misjudgment for fast walking, jogging and the like can be solved, and the system positioning accuracy is further improved.
Description
Technical field
The present invention relates to communication technical field, more particularly to a kind of zero-velocity curve walking to threshold smoothing self-adaptative adjustment
The projectional technique and device of person track.
Background technology
Indoors, in the environment such as jungle, global position system cannot realize positioning due to being blocked, in above-mentioned scene
Positioning application in, based on inertance element pedestrian track calculate scheme realized due to being affected by the external environment entirely
Scene is positioned, therefore increasingly causes the extensive concern of people.In the scheme that the reckoning of pedestrian track is carried out based on inertance element
In, it is zero-velocity curve algorithm that application is more.The interval acquisition motion view of zero-speed when the algorithm is landed using pedestrian foot
Measure and kinematic parameter is modified, suppress gyroscopic drift and improve positioning precision.Because zero-velocity curve algorithm is to zero-speed
The required precision of interval detection is very high, therefore zero-speed detection method directly determines the overall performance of system.
Traditional zero-speed detection method is based on the accelerometer after compensation and gyroscope measurement data, using joint threshold value
Mode judged, but because the threshold value for being adopted is for fixed empirical value, it is impossible to adapt to the various gestalt movements of people and cause
Precision is poor；Method based on Neyman-Pearson criterions estimated zero-speed state using Maximum Likelihood Estimation,
But realize that excessively complicated difficult is to obtain in a device practical application；At present the threshold adaptive method of adjustment of application is generally to people
Walking make a decision with running two states, and threshold value, but the ratio are arranged still to set in advance with fixed proportion, therefore to fast
There is the possibility of erroneous judgement in the complicated gait such as walk and jog, so as to cause precision to reduce.
The content of the invention
In view of above-mentioned analysis, the present invention is intended to provide a kind of zero-velocity curve pedestrian to threshold smoothing self-adaptative adjustment
The projectional technique and device of track, can fully or at least partially solve the above problems.
To solve the above problems, the present invention is mainly achieved through the following technical solutions：
One aspect of the present invention provides the projectional technique that a kind of threshold adaptive adjusts zero-speed detection pedestrian track, the party
Method includes：Acceleration information, angular velocity data and the magnetic field data of pedestrian foot are obtained, to the acceleration information, institute
State angular velocity data and the magnetic field data compensated, and the acceleration information after compensation is made energy calculation, low pass
Filter and adjudicate twice, determine the paces frequency of pedestrian；
According to the sampling interval of back to the angular velocity data after compensation, corresponding first angular speed energy is calculated equal
Value, and corresponding second angular speed average energy value is calculated to the angular velocity data after compensation according to the current sliding window for walking；
The corresponding proportionality coefficient of paces frequency is calculated according to calculated paces frequency；
Adaptive threshold is calculated according to the first angular speed average energy value and the product of the proportionality coefficient, according to
The zero-speed state that the second angular speed average energy value carries out pedestrian with the adaptive threshold judges.
Further, described pair compensation after acceleration information make energy calculation, LPF with adjudicate twice, it is determined that
The paces frequency of pedestrian, specifically includes：
Calculate acceleration energyWherein, ax,ay,azRespectively acceleration is in x-axis, y-axis and z-axis
Component；
To acceleration energy EgLPF is carried out, filtered acceleration energy E ' is obtainedg；
To the acceleration energy E ' after LPFgCrest and trough preliminary ruling are carried out, the ripple after preliminary ruling is obtained
Peak and trough sequence；
Second judgement is carried out to the trough after preliminary ruling, effective trough and invalid trough is determined；
To inverted paces frequency v for obtaining pedestrian in effective trough intervalf。
Further, the trough after preliminary ruling carries out second judgement, determines effective trough and invalid trough, concrete bag
Include：
Preliminary ruling crest is found around trough after preliminary ruling, if there is preliminary ruling crest, the ripple is judged
Paddy is effective trough；Otherwise, it is invalid trough.
Further, according to the sampling interval of back to the angular velocity data after compensation, calculate corresponding first jiao it is fast
Degree average energy value, and, to the angular speed number after compensation, calculate corresponding second angular speed energy according to according to the sliding window of current step
Amount average, specifically includes：
Calculate the angular speed modulus value of ith sample point | | wi| | it is：Wherein, wx,
wy,wzRespectively component of the angular speed in x-axis, y-axis and z-axis；
If (m, n) is the sampling interval of s steps, the angular speed average energy value for calculating s steps is：
Count s+1 step in ith sample point angular speed modulus value be | | wi| |, gyroscope noise variance isSliding window
Mouth width is W, and calculating is in sliding window (n, n+W-1) interior angle velocity energy average：
Further, self adaptation is calculated according to the first angular speed average energy value and the product of the proportionality coefficient
Threshold value, judges, specifically according to the zero-speed state that the second angular speed average energy value and the adaptive threshold carry out pedestrian
Including：
The motion state of pedestrian is done and is judged：As T=1, it is believed that be currently at zero-speed shape
State, proportionality coefficient Rp=k0·vf+α0, wherein, k0With α0For the empirical value of preset in advance.
Another aspect of the present invention additionally provides a kind of estimation device of adaptive threshold zero-speed detection pedestrian track, the dress
Put including：
First computing unit, it is right for obtaining acceleration information, angular velocity data and the magnetic field data of pedestrian foot
The acceleration information, the angular velocity data and the magnetic field data are compensated, and to the acceleration information after compensation
Make energy calculation, LPF with adjudicate twice, determine the paces frequency of pedestrian；
Second computing unit, to the angular velocity data after compensation, calculates corresponding for according to the sampling interval of back
First angular speed average energy value, and corresponding second is calculated to the angular velocity data after compensation according to the current sliding window for walking
Angular speed average energy value, according to calculated paces frequency the corresponding proportionality coefficient of paces frequency is calculated；
3rd computing unit, for being calculated according to the first angular speed average energy value and the product of the proportionality coefficient
To adaptive threshold, sentenced according to the zero-speed state that the second angular speed average energy value and the adaptive threshold carry out pedestrian
It is fixed.
Further, first computing unit is additionally operable to, and calculates acceleration energyWherein, ax,
ay,azRespectively component of the acceleration in x-axis, y-axis and z-axis；To acceleration energy EgLPF is carried out, after being filtered
Acceleration energy E 'g；To the acceleration energy E ' after LPFgCrest and trough preliminary ruling are carried out, preliminary ruling is obtained
Crest afterwards and trough sequence；Second judgement is carried out to the trough after preliminary ruling, effective trough and invalid trough is determined；To having
Inverted paces frequency v for obtaining pedestrian in effect trough intervalf。
Further, first computing unit is additionally operable to, and preliminary ruling ripple is found around the trough after preliminary ruling
Peak, if there is preliminary ruling crest, judges the trough as effective trough；Otherwise, it is invalid trough.
Further, first computing unit is additionally operable to, and calculates the angular speed modulus value of ith sample point | | wi| | it is：Wherein, wx,wy,wzRespectively component of the angular speed in x-axis, y-axis and z-axis；If (m,
N) be s step sampling interval, calculate s step angular speed average energy value be：Count in s+1 steps
Ith sample point angular speed modulus value be | | wi| |, gyroscope noise variance isSliding window width is W, calculates and is sliding
Window (n, n+W-1) interior angle velocity energy average is：
Further, the 3rd computing unit is additionally operable to, and the motion state of pedestrian is done and is judged：As T=1, it is believed that be currently at zero-speed state, proportionality coefficient Rp=k0·vf+α0, wherein, k0With
α0For the empirical value of preset in advance.
The present invention has the beneficial effect that：
The present invention obtains reliable initial data, the acceleration after to compensation by compensating to initial data
Data carry out LPF and second judgement, carry out the extraction of paces frequency, then that paces frequency is diagonal as input quantity
The threshold value of velocity energy average is adjusted, so as to reach the purpose of threshold smoothing self-adaptative adjustment.With threshold value general at present
Self-adapting regulation method is compared, and can carry out threshold smoothing adjustment, so as to adapt to the action of all kinds of paces frequencies of pedestrian, is overcome
The problem that situations such as to hurrying up and jogging judges by accident, so as to improving system accuracy.
Other features and advantages of the present invention will illustrate in the following description, and partial become from specification
It is clear that or being understood by implementing the present invention.The purpose of the present invention and other advantages can by the specification write,
Specifically noted structure is realizing and obtain in claims and accompanying drawing.
Description of the drawings
Fig. 1 is a kind of reckoning of zero-velocity curve pedestrian track to threshold smoothing self-adaptative adjustment of the embodiment of the present invention
The schematic flow sheet of method；
Fig. 2 for the embodiment of the present invention another kind pushing away to the zero-velocity curve pedestrian track of threshold smoothing self-adaptative adjustment
The schematic flow sheet of calculation method；
Fig. 3 is pushed away for another zero-velocity curve pedestrian track to threshold smoothing self-adaptative adjustment of the embodiment of the present invention
The schematic flow sheet of calculation method；
Fig. 4 is the trajectory diagram moved along sports ground obtained using the methods described of the embodiment of the present invention；
Fig. 5 is the trajectory diagram that the another kind obtained using the methods described of the embodiment of the present invention is moved along sports ground；
Fig. 6 is a kind of reckoning of zero-velocity curve pedestrian track to threshold smoothing self-adaptative adjustment of the embodiment of the present invention
The structural representation of device.
Specific embodiment
Below in conjunction with the accompanying drawings specifically describing the preferred embodiments of the present invention, wherein, accompanying drawing constitutes the application part, and
It is used to together with embodiments of the present invention explain the principle of the present invention.For purpose of clarity and simplification, when it may make the present invention
Theme it is smudgy when, illustrating in detail for known function and structure in device described herein will be omitted.
The invention provides a kind of projectional technique of the zero-velocity curve pedestrian track to threshold smoothing self-adaptative adjustment, this
Invention obtains reliable initial data by compensating to initial data, and the acceleration information after to compensation carries out low
Pass filter and second judgement, carry out the extraction of paces frequency, then that paces frequency is equal as input quantity angular velocity energy
The threshold value of value is adjusted, so as to reach the purpose of threshold adaptive adjustment.With threshold adaptive method of adjustment general at present
Compare, threshold smoothing adjustment can be carried out, so as to adapt to the action of all kinds of paces frequencies of pedestrian, overcome to hurrying up and jogging
Situations such as the problem judged by accident, so as to improve system accuracy.Below in conjunction with accompanying drawing and several embodiments, the present invention is carried out
Further describe.It should be appreciated that specific embodiment described herein only limits this to explain the present invention, not
It is bright.
Embodiments provide a kind of reckoning of the zero-velocity curve pedestrian track to threshold smoothing self-adaptative adjustment
Method, referring to Fig. 1, the method includes：
S101, the acceleration information for obtaining pedestrian foot, angular velocity data and magnetic field data, to the acceleration number of degrees
Compensate according to, the angular velocity data and the magnetic field data, and the acceleration information after compensation is made energy calculation,
LPF is adjudicated with twice, determines the paces frequency of pedestrian；
S102, according to the sampling interval of back to the angular velocity data after compensation, calculate corresponding first angular speed energy
Amount average, and corresponding second angular speed energy is calculated equal to the angular velocity data after compensation according to the current sliding window for walking
Value；
S103, the corresponding proportionality coefficient of paces frequency is calculated according to calculated paces frequency；
S104, adaptive thresholding is calculated according to the first angular speed average energy value and the product of the proportionality coefficient
Value, judges according to the zero-speed state that the second angular speed average energy value and the adaptive threshold carry out pedestrian.
That is, the present invention obtains reliable initial data, after to compensation by compensating to initial data
Acceleration information carry out LPF and second judgement, the extraction of paces frequency is carried out, then using paces frequency as defeated
The threshold value for entering amount angular velocity average energy value is adjusted, so as to reach the purpose of threshold adaptive adjustment.It is general with current
Threshold adaptive method of adjustment is compared, and can carry out threshold smoothing adjustment, so as to adapt to the action of all kinds of paces frequencies of pedestrian,
The problem that situations such as overcoming to hurrying up and jogging is judged by accident, so as to improve system accuracy.
Specifically, the embodiment of the present invention is come the acceleration number of degrees to obtaining pedestrian foot by bit alignment error compensation
Compensate according to, angular velocity data and magnetic field data.Certainly, those skilled in the art can also by other methods come
Each data to obtaining are compensated.
Be embodied as being that the acceleration information to after compensation of the present invention makes energy calculation, LPF with twice
Judgement, determines the paces frequency of pedestrian, specifically includes：
Calculate acceleration energy(wherein, ax,ay,azRespectively acceleration is in x-axis, y-axis and z-axis
Component)；
To acceleration energy EgLPF is carried out, filtered acceleration energy E ' is obtainedg；
To the acceleration energy E ' after LPFgCrest and trough preliminary ruling are carried out, the ripple after preliminary ruling is obtained
Peak and trough sequence；
Second judgement is carried out to the trough after preliminary ruling, effective trough and invalid trough is determined；
To inverted paces frequency v for obtaining pedestrian in effective trough intervalf。
Trough of the embodiment of the present invention after preliminary ruling carries out second judgement, determines effective trough and invalid trough, tool
Body includes：
Preliminary ruling crest is found around trough after preliminary ruling, if there is preliminary ruling crest, the ripple is judged
Paddy is effective trough；Otherwise, it is invalid trough.
According to the sampling interval of back to the angular velocity data after compensation described in the embodiment of the present invention, corresponding the is calculated
One angular speed average energy value, and, to the angular speed number after compensation, calculate corresponding second jiao according to according to the sliding window of current step
Velocity energy average, specifically includes：
Calculate the angular speed modulus value of ith sample point | | wi| | it is：(wherein, wx,
wy,wzRespectively component of the angular speed in x-axis, y-axis and z-axis)；
If (m, n) is the sampling interval of s steps, the angular speed average energy value for calculating s steps is：
Count s+1 step in ith sample point angular speed modulus value be | | wi| |, gyroscope noise variance isSliding window
Mouth width is W, and calculating is in sliding window (n, n+W-1) interior angle velocity energy average：
Calculated according to the first angular speed average energy value and the product of the proportionality coefficient described in the embodiment of the present invention
To adaptive threshold, sentenced according to the zero-speed state that the second angular speed average energy value and the adaptive threshold carry out pedestrian
It is fixed, specifically include：
The motion state of pedestrian is done and is judged：As T=1, it is believed that be currently at zero-speed shape
State, proportionality coefficient Rp=k0·vf+α0, wherein, k0With α0For the empirical value of preset in advance.
As shown in Fig. 2 the present invention provides a kind of pedestrian track projectional technique of adaptive threshold zero-speed detection, the method
Key step include：MEMS (MEMS) inertia sensing element, including accelerometer, gyro are fixed in pedestrian foot
Instrument and magnetometer；After system starts, start work point (origin) is selected first, set original state；Then obtain by
The sensing data that the MEMS sensor is provided, and compensate；Subsequently, using adaptive threshold zero-speed detection method, sentence
Whether pedestrian is determined in zero-speed state；If so, then based on EKF system mode is corrected, and is
System state recursion；If it is not, then directly carrying out system mode recursion；Circulation perform pedestrian's zero-speed state judge, based on expansion card
Kalman Filtering is updated to system mode, three steps of system mode recursion；Finally, positioning result is exported.
As shown in figure 3, pedestrian's zero-speed condition judgement method provided by the present invention, it is necessary first to carry out cadence calculating,
Key step includes：
Calculate acceleration energy
To acceleration energy EgLPF is carried out, E ' is obtainedg；
To the E ' after LPFgCarry out crest to adjudicate with trough, obtain the crest after preliminary ruling and trough sequence；
Second judgement is carried out to the crest after preliminary ruling and trough sequence, decision method is：Ripple after preliminary ruling
Preliminary ruling crest is found around paddy, if there is preliminary ruling crest, judges this trough as effective trough；Otherwise, it is considered as nothing
Effect trough；
Effective trough is spaced inverted, you can obtain cadence vf。
It is calculated cadence vfAfterwards, it is with reference to calculating threshold percentage coefficient with it；And based on angular speed average energy value, with
Back angular speed average energy value is threshold value with the product of the proportionality coefficient, carries out zero-speed state-detection, and concrete steps include：
Calculate the angular speed modulus value of ith sample point | | wi| | it is：
If (m, n) is the sampling interval of s steps, the angular speed average energy value for calculating s steps is：
Count s+1 step in ith sample point angular speed modulus value be | | wi| |, gyroscope noise variance isSliding window
Mouth width is W, and calculating is in sliding window (n, n+W-1) interior angle velocity energy average：
Calculate proportionality coefficient Rp：Rp=k0·vf+α0(wherein, k0With α0For the empirical value of preset in advance)；
Motion state is done and is judged：
As T=1, it is believed that be currently at zero-speed state.
On the whole, the method described in the embodiment of the present invention includes：
MEMS sensor is fixed in pedestrian foot, the raw sensories such as acceleration, angular speed and the magnetic field of foot are obtained
Data；
The raw sensory data need to be compensated through correlative compensation method；
Acceleration information after compensation need to make energy calculation, LPF and judging process twice, to determine pedestrian
Paces frequency；
Angular velocity data after compensation need to calculate corresponding angular speed average energy value according to the sampling interval of back；
Angular velocity data after compensation needs the sliding window according to current step, calculates corresponding angular speed average energy value；
According to the paces frequency for being calculated, corresponding proportionality coefficient is calculated；
Adaptive threshold is calculated according to back angular speed average energy value with the product of the proportionality coefficient；
The angular speed average energy value and the adaptive threshold in sliding window is more currently walked, the zero-speed of pedestrian is carried out
State judges.
Fig. 5 and Fig. 6 is the result figure that calculated to pedestrian track using the method that provides of the present invention, present invention employing
The modules of MTI 100 of Dutch Xsens companies have carried out two groups of walk tests.Fig. 5 is corresponding be the place of experiment be Wei Haibei
The basketball court of foreign electrically Group Plc, along the walking of basketball court white line, about 505 meters of total distance returns to starting point, all the time
Point tolerance is 0.87 meter, and positioning precision is 0.17%.Fig. 6 is corresponding be the place of experiment be Shandong University at Weihai stadium,
Along runway walking, starting point, total distance are returned to>800 meters, all the time point tolerance is 2.89 meters, and positioning precision is 0.36%.
On the whole, by the application of MEMS sensor, can indoors, the global position system such as jungle blocked and lost
Positioning is realized in the environment of effect；Low pass is carried out by compensating to sensor raw data, to the acceleration information after compensation
The processes such as filtering and second judgement, angular speed average energy value are calculated, threshold adaptive adjustment, can be in pedestrian's walking, race
The accurate judgement that zero-speed state is realized under all kinds of behavior conditions such as walk, hurry up, jogging, so as to effectively improve positioning precision.
Corresponding with the method shown in Fig. 1, the embodiment of the present invention additionally provides a kind of to threshold smoothing self-adaptative adjustment
Zero-velocity curve pedestrian track estimation device, referring to Fig. 6, the device includes：
First computing unit, it is right for obtaining acceleration information, angular velocity data and the magnetic field data of pedestrian foot
The acceleration information, the angular velocity data and the magnetic field data are compensated, and to the acceleration information after compensation
Make energy calculation, LPF with adjudicate twice, determine the paces frequency of pedestrian；
Second computing unit, to the angular velocity data after compensation, calculates corresponding for according to the sampling interval of back
First angular speed average energy value, and corresponding second is calculated to the angular velocity data after compensation according to the current sliding window for walking
Angular speed average energy value, according to calculated paces frequency the corresponding proportionality coefficient of paces frequency is calculated；
3rd computing unit, for being calculated according to the first angular speed average energy value and the product of the proportionality coefficient
To adaptive threshold, sentenced according to the zero-speed state that the second angular speed average energy value and the adaptive threshold carry out pedestrian
It is fixed.
When being embodied as, the first computing unit is additionally operable to described in the embodiment of the present invention, calculates acceleration energy(wherein, ax,ay,azRespectively component of the acceleration in x-axis, y-axis and z-axis)；To acceleration energy Eg
LPF is carried out, filtered acceleration energy E ' is obtainedg；To the acceleration energy E ' after LPFgCarry out crest and ripple
Paddy preliminary ruling, obtains the crest after preliminary ruling and trough sequence；Second judgement is carried out to the trough after preliminary ruling, it is determined that
Effective trough and invalid trough；To inverted paces frequency v for obtaining pedestrian in effective trough intervalf。
Further, the first computing unit is additionally operable to described in the embodiment of the present invention, seeks around the trough after preliminary ruling
Preliminary ruling crest is looked for, if there is preliminary ruling crest, judges the trough as effective trough；Otherwise, it is invalid trough.
Further, the first computing unit is additionally operable to described in the embodiment of the present invention, calculates the angular speed mould of ith sample point
Value | | wi| | it is：(wherein, wx,wy,wzRespectively angular speed is in x-axis, y-axis and z-axis
Component)；If (m, n) is the sampling interval of s steps, the angular speed average energy value for calculating s steps is：Count s+1 step in ith sample point angular speed modulus value be | | wi| |, gyroscope noise variance isSliding window width is W, and calculating is in sliding window (n, n+W-1) interior angle velocity energy average：
When being embodied as, the 3rd computing unit is additionally operable to described in the embodiment of the present invention, the motion state of pedestrian is done and is sentenced
It is fixed：As T=1, it is believed that be currently at zero-speed state, proportionality coefficient Rp=k0·vf+α0, wherein,
k0With α0For the empirical value of preset in advance.
The present embodiment provides a kind of pedestrian's dead reckoning system of self adaptation point of zero velocity detection, by being fixed on pedestrian
The MEMS inertia sensing modules of foot are positioned, wherein, described MEMS inertia sensings module include three axis accelerometer, three
Axle gyroscope and three axle magnetometer.
When being embodied as, the present invention may also comprise inertia sensing module, initial to the device that the pedestrian track calculates
State setting module, sensing data compensating module, judge module, estimation module, system recursion module and output module.
Method according to the present invention, comprises the steps：
Described MEMS inertia sensing modules are fixed on into the foot of pedestrian；
The original state setup module is used to arrange system initial state；
The sensing data compensating module enters according to set compensation data method to the initial data of inertia sensing module
Row compensation, and three-dimensional correction is carried out to magnetometer data using the least square method assumed based on ellipsoid；
The judge module is used to judge the motion state of pedestrian, first to determine if in zero-speed state
Paces frequency calculating is first carried out, concrete steps include：1) acceleration energy is calculated2) to acceleration energy Eg
LPF is carried out, E ' is obtainedg；2) to the E ' after LPFgCarry out crest to adjudicate with trough, obtain the crest after preliminary ruling
With trough sequence；3) second judgement is carried out to the crest after preliminary ruling and trough sequence, decision method is：After preliminary ruling
Preliminary ruling crest is found around trough, if there is preliminary ruling crest, judges this trough as effective trough；Otherwise, it is considered as nothing
Effect trough；4) it is inverted to effective trough interval, obtain paces frequency vf.After obtaining paces frequency, carry out and based on angular speed
Average energy value, and with the product of back angular speed average energy value and the proportionality coefficient as threshold value, zero-speed state-detection is carried out,
Concrete steps include：1) the angular speed modulus value of ith sample point is calculated | | wi| | it is：
2) sampling interval that (m, n) is s steps is set, the angular speed average energy value for calculating s steps is：3) count
S+1 step in ith sample point angular speed modulus value be | | wi| |, gyroscope noise variance isSliding window width is W,
Calculate and be in sliding window (n, n+W-1) interior angle velocity energy average：4) proportionality coefficient R is calculatedp：
Rp=k0·vf+α0(wherein, k0With α0For the empirical value of preset in advance).Finally, motion state is done and judges：As T=1, it is believed that be currently at zero-speed state.
The estimation module estimates that concrete grammar is as follows based on extended Kalman filter to positional information error：
The system state equation for setting up Kalman filtering isSelecting system state
Variable isWherein：
φE、φN、φUFor pitching angle error, roll angle error and azimuth angle error；δVE、δVN、δVURespectively east orientation, north orientation and day to
Velocity error；δ λ are latitude, longitude error；εX、εY、εZRespectively gyro northeast day to drift；
Respectively accelerometer northeast day to biasing；W (t)=[wgx wgy wgz wax way waz]TWhite Gaussian noise, and E [W (t)]
=0, cov [Wk,Wj]=E [WkWj T]Qkδkj(QkFor system noise variance matrix).
Wherein,
FAFor 8 × 8 rank matrixes, wherein nonzero element is as follows：
F (4,2)=- fU
F (4,3)=fN
F (5,1)=fU
F (5,3)=- fE
F (6,1)=- fN
F (6,2)=fE
2) observational equation is
yk=Hzk+νk
Wherein, H is observing matrix, νkFor noise matrix.
When the zero-speed detection scheme provided with the present invention detects zero-speed state, using magnetometer course angle, zero-speed
Degree and zero angular velocity are modified as observed quantity to system mode, and the observed quantity of system is zk=[Δ φU,δVE,δVN,δVU,
εX,εY,εZ], observing matrix is
When non-zero-speed state is detected, system mode is modified merely with magnetometer course angle information, system
Observed quantity is that observing matrix is
H=[[0,0,1] 01×3 01×2 01×3 01×3]。
The above, the only present invention preferably specific embodiment, but protection scope of the present invention is not limited thereto,
Any those familiar with the art the invention discloses technical scope in, the change or replacement that can be readily occurred in,
All should be included within the scope of the present invention.Therefore, protection scope of the present invention should be with the protection model of claims
Enclose and be defined.
Claims (10)
1. a kind of projectional technique of the zero-velocity curve pedestrian track to threshold smoothing self-adaptative adjustment, it is characterised in that include：
Acceleration information, angular velocity data and the magnetic field data of pedestrian foot are obtained, to the acceleration information, the angle
Speed data and the magnetic field data are compensated, and the acceleration information after compensation is made energy calculation, LPF
With adjudicate twice, determine the paces frequency of pedestrian；
According to the sampling interval of back to the angular velocity data after compensation, corresponding first angular speed average energy value is calculated, and
According to the current sliding window for walking to the angular velocity data after compensation, corresponding second angular speed average energy value is calculated；
The corresponding proportionality coefficient of paces frequency is calculated according to calculated paces frequency；
Adaptive threshold is calculated according to the first angular speed average energy value and the product of the proportionality coefficient, according to described
The zero-speed state that second angular speed average energy value carries out pedestrian with the adaptive threshold judges.
2. projectional technique according to claim 1, it is characterised in that the acceleration information after described pair of compensation carries out energy
Calculate, LPF and adjudicate twice, determine the paces frequency of pedestrian, specifically include：
Calculate acceleration energyWherein, ax,ay,azRespectively acceleration x-axis, y-axis and z-axis point
Amount；
To acceleration energy EgLPF is carried out, filtered acceleration energy E ' is obtainedg；
To the acceleration energy E ' after LPFgCrest and trough preliminary ruling are carried out, the crest after preliminary ruling and ripple is obtained
Paddy sequence；
Second judgement is carried out to the trough after preliminary ruling, effective trough and invalid trough is determined；
To inverted paces frequency v for obtaining pedestrian in effective trough intervalf。
3. projectional technique according to claim 2, it is characterised in that the trough after preliminary ruling carries out second judgement,
Determine effective trough and invalid trough, specifically include：
Around trough after preliminary ruling find preliminary ruling crest, if there is preliminary ruling crest, judge the trough as
Effective trough；Otherwise, it is invalid trough.
4. projectional technique according to claim 1, it is characterised in that according to the sampling interval of back to the angle after compensation
Speed data, calculates corresponding first angular speed average energy value, and according to the sliding window according to current step to the angle speed after compensation
The number of degrees, calculate corresponding second angular speed average energy value, specifically include：
Calculate the angular speed modulus value of ith sample point | | wi| | it is：Wherein, wx,wy,wz
Respectively component of the angular speed in x-axis, y-axis and z-axis；
If (m, n) is the sampling interval of s steps, the angular speed average energy value for calculating s steps is：
Count s+1 step in ith sample point angular speed modulus value be | | wi| |, gyroscope noise variance isSliding window width
Spend for W, calculating is in sliding window (n, n+W-1) interior angle velocity energy average：
5. projectional technique according to claim 4, it is characterised in that according to the first angular speed average energy value with it is described
The product of proportionality coefficient is calculated adaptive threshold, is entered according to the second angular speed average energy value and the adaptive threshold
The zero-speed state of row pedestrian judges, specifically includes：
The motion state of pedestrian is done and is judged：As T=1, it is believed that be currently at zero-speed state,
Proportionality coefficient Rp=k0·vf+α0, wherein, k0With α0For the empirical value of preset in advance.
6. a kind of estimation device of the zero-velocity curve pedestrian track to threshold smoothing self-adaptative adjustment, it is characterised in that include：
First computing unit, for obtaining acceleration information, angular velocity data and the magnetic field data of pedestrian foot, to described
Acceleration information, the angular velocity data and the magnetic field data are compensated, and the acceleration information after compensation is carried out
Energy balane, LPF are adjudicated with twice, determine the paces frequency of pedestrian；
Second computing unit, to the angular velocity data after compensation, corresponding first is calculated for according to the sampling interval of back
Angular speed average energy value, and according to the sliding window of current step to the angular velocity data after compensation, calculate corresponding second jiao it is fast
Degree average energy value, according to calculated paces frequency the corresponding proportionality coefficient of paces frequency is calculated；
3rd computing unit, for being calculated certainly according to the first angular speed average energy value and the product of the proportionality coefficient
Threshold value is adapted to, is judged according to the zero-speed state that the second angular speed average energy value and the adaptive threshold carry out pedestrian.
7. estimation device according to claim 6, it is characterised in that
First computing unit is additionally operable to, and calculates acceleration energyWherein, ax,ay,azRespectively accelerate
Spend the component in x-axis, y-axis and z-axis；To acceleration energy EgLPF is carried out, filtered acceleration energy E ' is obtainedg；
To the acceleration energy E ' after LPFgCrest and trough preliminary ruling are carried out, the crest after preliminary ruling and trough is obtained
Sequence；Second judgement is carried out to the trough after preliminary ruling, effective trough and invalid trough is determined；Effective trough interval is taken down
Number obtains paces frequency v of pedestrianf。
8. estimation device according to claim 7, it is characterised in that
First computing unit is additionally operable to, and preliminary ruling crest is found around the trough after preliminary ruling, if existing preliminary
Judgement crest, then judge the trough as effective trough；Otherwise, it is invalid trough.
9. estimation device according to claim 6, it is characterised in that
First computing unit is additionally operable to, and calculates the angular speed modulus value of ith sample point | | wi| | it is：Wherein, wx,wy,wzRespectively component of the angular speed in x-axis, y-axis and z-axis；If (m,
N) be s step sampling interval, calculate s step angular speed average energy value be：Count in s+1 steps
Ith sample point angular speed modulus value be | | wi| |, gyroscope noise variance isSliding window width is W, calculates and is sliding
Window (n, n+W-1) interior angle velocity energy average is：
10. estimation device according to claim 9, it is characterised in that
3rd computing unit is additionally operable to, and the motion state of pedestrian is done and is judged：Work as T=1
When, it is believed that it is currently at zero-speed state, proportionality coefficient Rp=k0·vf+α0, wherein, k0With α0For the empirical value of preset in advance.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611196322.3A CN106643713B (en) | 2016-12-22 | 2016-12-22 | Estimation method and device for zero-speed correction pedestrian locus for smooth and adaptive adjustment of threshold |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611196322.3A CN106643713B (en) | 2016-12-22 | 2016-12-22 | Estimation method and device for zero-speed correction pedestrian locus for smooth and adaptive adjustment of threshold |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106643713A true CN106643713A (en) | 2017-05-10 |
| CN106643713B CN106643713B (en) | 2021-01-26 |
Family
ID=58834944
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611196322.3A Active CN106643713B (en) | 2016-12-22 | 2016-12-22 | Estimation method and device for zero-speed correction pedestrian locus for smooth and adaptive adjustment of threshold |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106643713B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110108301A (en) * | 2019-05-14 | 2019-08-09 | 苏州大学 | Modulus value detects moving base robust alignment methods |
| CN110455316A (en) * | 2019-08-27 | 2019-11-15 | 哈尔滨工业大学 | A kind of adaptive zero-speed section detecting method |
| CN111221420A (en) * | 2020-01-13 | 2020-06-02 | 深圳大学 | 2D movement track identification method and system based on smart watch |
| CN114279441A (en) * | 2021-12-15 | 2022-04-05 | 中国科学院深圳先进技术研究院 | Zero-speed interval detection method, pedestrian navigation system and storage medium |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080120062A1 (en) * | 2006-10-31 | 2008-05-22 | Samsung Electronics Co., Ltd. | Step length estimation method and portable terminal for the same |
| CN103644910A (en) * | 2013-11-22 | 2014-03-19 | 哈尔滨工程大学 | Personal autonomous navigation system positioning method based on segment RTS smoothing algorithm |
| CN104757976A (en) * | 2015-04-16 | 2015-07-08 | 大连理工大学 | Human gait analyzing method and system based on multi-sensor fusion |
| CN105547291A (en) * | 2015-12-31 | 2016-05-04 | 长沙格致电子科技有限公司 | Self-adaptive static detection method for indoor personnel self-localization system |
-
2016
- 2016-12-22 CN CN201611196322.3A patent/CN106643713B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080120062A1 (en) * | 2006-10-31 | 2008-05-22 | Samsung Electronics Co., Ltd. | Step length estimation method and portable terminal for the same |
| CN103644910A (en) * | 2013-11-22 | 2014-03-19 | 哈尔滨工程大学 | Personal autonomous navigation system positioning method based on segment RTS smoothing algorithm |
| CN104757976A (en) * | 2015-04-16 | 2015-07-08 | 大连理工大学 | Human gait analyzing method and system based on multi-sensor fusion |
| CN105547291A (en) * | 2015-12-31 | 2016-05-04 | 长沙格致电子科技有限公司 | Self-adaptive static detection method for indoor personnel self-localization system |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110108301A (en) * | 2019-05-14 | 2019-08-09 | 苏州大学 | Modulus value detects moving base robust alignment methods |
| CN110455316A (en) * | 2019-08-27 | 2019-11-15 | 哈尔滨工业大学 | A kind of adaptive zero-speed section detecting method |
| CN111221420A (en) * | 2020-01-13 | 2020-06-02 | 深圳大学 | 2D movement track identification method and system based on smart watch |
| CN111221420B (en) * | 2020-01-13 | 2021-07-30 | 深圳大学 | 2D movement track identification method and system based on smart watch |
| CN114279441A (en) * | 2021-12-15 | 2022-04-05 | 中国科学院深圳先进技术研究院 | Zero-speed interval detection method, pedestrian navigation system and storage medium |
| CN114279441B (en) * | 2021-12-15 | 2023-12-29 | 中国科学院深圳先进技术研究院 | Zero-speed interval detection method, pedestrian navigation system and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106643713B (en) | 2021-01-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104061934B (en) | Pedestrian indoor position tracking method based on inertial sensor | |
| CN104406586B (en) | Pedestrian navigation apparatus and method based on inertial sensor | |
| CN104757976B (en) | A kind of Human Body Gait Analysis method and system based on Multi-sensor Fusion | |
| CN105737823B (en) | A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF | |
| CN109827577A (en) | High-precision inertial navigation location algorithm based on motion state detection | |
| CN106705968A (en) | Indoor inertial navigation algorithm based on posture recognition and step length model | |
| CN108106635A (en) | Inertia defends the anti-interference posture course calibration method of long endurance for leading integrated navigation system | |
| CN103727941B (en) | Based on the volume Kalman nonlinear combination air navigation aid of carrier system speeds match | |
| CN107478223A (en) | A kind of human body attitude calculation method based on quaternary number and Kalman filtering | |
| CN106643713A (en) | Zero-velocity correct walker trajectory estimation method and device for smooth and self-adaptive threshold value adjustment | |
| CN104613965B (en) | A kind of step-by-step movement pedestrian navigation method based on bidirectional filtering smoothing technique | |
| CN106153073B (en) | A kind of nonlinear initial alignment method of full posture Strapdown Inertial Navigation System | |
| CN107289930A (en) | Pure inertia automobile navigation method based on MEMS Inertial Measurement Units | |
| CN106643715A (en) | Indoor inertial navigation method based on bp neural network improvement | |
| CN109000640A (en) | Vehicle GNSS/INS Combinated navigation method based on discrete Grey Neural Network Model | |
| CN107490378A (en) | A kind of indoor positioning based on MPU6050 and smart mobile phone and the method for navigation | |
| CN104132662A (en) | Closed-loop Kalman filter inertial positioning method based on zero velocity update | |
| CN109459028A (en) | A kind of adaptive step estimation method based on gradient decline | |
| CN103674064B (en) | Initial calibration method of strapdown inertial navigation system | |
| CN106996780A (en) | A kind of course error modification method and device and magnetic field detection method and device | |
| CN108007477A (en) | A kind of inertia pedestrian's Positioning System Error suppressing method based on forward and reverse filtering | |
| Li et al. | A robust pedestrian navigation algorithm with low cost IMU | |
| CN106153069A (en) | Attitude rectification apparatus and method in autonomous navigation system | |
| CN103644910A (en) | Personal autonomous navigation system positioning method based on segment RTS smoothing algorithm | |
| CN108180923A (en) | A kind of inertial navigation localization method based on human body odometer |
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 |