CN110398245A - The indoor pedestrian navigation Attitude estimation method of formula Inertial Measurement Unit is worn based on foot - Google Patents
The indoor pedestrian navigation Attitude estimation method of formula Inertial Measurement Unit is worn based on foot Download PDFInfo
- Publication number
- CN110398245A CN110398245A CN201910615908.6A CN201910615908A CN110398245A CN 110398245 A CN110398245 A CN 110398245A CN 201910615908 A CN201910615908 A CN 201910615908A CN 110398245 A CN110398245 A CN 110398245A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- posture
- pedestrian
- measurement unit
- formula
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- 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)
Abstract
The present invention provides a kind of indoor pedestrian navigation Attitude estimation methods that formula Inertial Measurement Unit is worn based on foot, first, Inertial Measurement Unit is installed on pedestrian's foot, initialize initial attitude of the Inertial Measurement Unit under navigational coordinate system, then judge whether pedestrian remains static according to the output of inertial sensor, when pedestrian is in nonstatic state, posture renewal is carried out using inertial navigation algorithm, recursion obtains current posture, when pedestrian remains static, dynamic model is constructed according to inertial navigation algorithm posture renewal equation, accelerometer output constructs measurement model with gravity relationship, current posture is updated by Kalman filter under quaternary number frame.The present invention utilizes the measurement construct Attitude estimation observed quantity of Still time accelerometer, and Kalman filter progress data fusion is executed under four element frames and obtains accurate posture information, realizes and provides the technical effect of accurate posture information.
Description
Technical field
The present invention relates to pedestrian navigation technical field more particularly to a kind of indoor rows that formula Inertial Measurement Unit is worn based on foot
People's navigation Attitude estimation method.
Background technique
" pedestrian navigation " refers on the basis of map, provides necessary position, directional information, and guidance pedestrian reaches set
Target.In the intelligent electronic device enriched constantly, pedestrian navigation has become one item critical function (Gao De in such as smart phone
The pedestrian navigation service of the offers such as map, Baidu map), greatly improve people goes out line efficiency.
In outdoor, by Global Navigation Satellite System (Global Navigation Satellite System, GNSS)
Accurate location information can be obtained, in conjunction with other direction sensors (such as gyroscope), can realize that accurate pedestrian leads
Boat.However indoors, blocking due to building structure and wall, GNSS signal cannot estimate effective position by deep fades
Information.Therefore, indoor pedestrian navigation difficulties are more, need to develop special technology, are current location navigation necks both at home and abroad
One of the hot spot of domain research.
In the prior art, available indoor location estimation technique is broadly divided into the method for view-based access control model, based on wireless communication
Number method and the method based on inertial sensor etc..The problems such as in view of cost, power consumption, computational complexity, formula is worn based on foot
The method of Inertial Measurement Unit (Inertial Measurement Unit, IMU) is widely used in indoor pedestrian navigation position
Estimation.How to obtain accurate directional information is the Major Difficulties of indoor pedestrian navigation.By angular-rate sensor built in IMU
It is a kind of economic, simple method that measurement, which carries out direction estimation, but there are direction drifting problem, needs to carry out not direction estimation
Disconnected correction, to limit direction drift.Current solution includes zero acceleration more new algorithm, magnetic heading aided algorithm etc..
At least there is following technology in implementing the present invention, it may, the method for finding the prior art in present inventor
Problem:
Zero acceleration more new algorithm depends on used attitude error model, only under terrestrial coordinate system effectively；Magnetic
Course aided algorithm needs accurate course observation, but since indoor geomagnetic noise is serious, the boat obtained by magnetic survey
Large error will be present to estimation.In addition to this, these solutions needs carry out under terrestrial coordinate system, substantially increase
The initial work of navigation system；Also, these schemes generally only provide course information, and cannot provide accurate three-dimensional
Information (i.e. posture).It navigates for some special indoor pedestrian navigation applications, such as fire fighter, posture information is conducive to supervise
The walking states (stand, lie low, squatting down) for the personnel that are navigated are surveyed, to speculate whether it is safe.
It follows that method in the prior art there is technical issues that be difficult to provide it is accurate.
Summary of the invention
To solve the problems, such as that current indoor pedestrian navigation system is difficult to provide accurate posture information, the present invention provides a kind of base
The indoor pedestrian navigation Attitude estimation method of Formulas I MU is worn in foot, this method exports structure using the accelerometer of pedestrian's stationary state
Direction observed quantity is built, then Kalman filter is executed under quaternary number frame and the Attitude estimation of IMU strapdown inertial is changed
Just.
The present invention provides the indoor pedestrian navigation Attitude estimation methods that Formulas I MU is worn based on foot, comprising:
Step S1: Inertial Measurement Unit is installed on pedestrian's foot, determines navigational coordinate system, to the zero of Inertial Measurement Unit
Inclined error carries out on-line proving, initializes initial attitude of the Inertial Measurement Unit under navigational coordinate system, wherein navigational coordinate system
Z axis it is perpendicular to the ground；
Step S2: judging whether pedestrian remains static according to the output of inertial sensor, if pedestrian is in non-quiet
Only state executes step S3, no to then follow the steps S4；
Step S3: according to the output of initial attitude and three-axis gyroscope, carrying out posture renewal using inertial navigation algorithm,
Recursion obtains current posture, wherein the measurement of three-axis gyroscope is opposite as Inertial Measurement Unit under sensor coordinate system
In the angular velocity of rotation of navigational coordinate system；
Step S4: dynamic model, accelerometer output are constructed according to inertial navigation algorithm posture renewal equation and closed with gravity
Series structure measurement model is updated current posture by Kalman filter under quaternary number frame.
In one embodiment, step S1 is specifically included:
Step S1.1: Inertial Measurement Unit is installed on pedestrian's foot, customized three-dimensional cartesian coordinate system is sat as navigation
Mark system, it is ensured that the Z axis of navigational coordinate system is perpendicular to the ground；
Step S1.2: for three-axis gyroscope, with each average value axially exported in lower section of preset time of stationary state
Zero offset error as the axis.For three axis accelerometer, pass through the lower four different locations the preceding paragraph preset time of stationary state
The output of interior accelerometer is demarcated, shown in calibration formula such as formula (1),
Wherein, bx,by,bzRespectively zero offset error in three axis accelerometer X, Y, Z axis, xi、yi、zi, i=1,2,3,4, point
The average value that X, Y, Z are exported on i-th of position, m are not indicatedi, i=1,2,3,4 indicate that each axis exports average value on i-th of position
Quadratic sum；
Step S1.3: at the initial navigation moment, adjusting the position of Inertial Measurement Unit, sits Inertial Measurement Unit sensor
Mark system is aligned with navigational coordinate system, to obtain initial attitude of the Inertial Measurement Unit under navigational coordinate system, wherein initial appearance
The form of state is q0=[1,0,0,0]T。
In one embodiment, step S2 is specifically included:
Step S2.1: judging whether the sequence total length of current Inertial Measurement Unit output is greater than or equal to preset length N,
If it is, going to step S2.2, judge whether pedestrian remains static；
Step S2.2: current three-axis gyroscope measurement vector field homoemorphism is calculated | | yk- b | |, wherein ykFor current three axis accelerometer
Instrument measures vector, and b is the zero bias vector of three-axis gyroscope obtained in step S1, and judges | | yk- b | | whether it is greater than setting
First threshold thm, if it is greater, then determining that pedestrian is in nonstatic state at current time, otherwise, execute step S2.3；
Step S2.3: the variance var of the gyroscope measurement vector mould in fixed length window, calculation formula such as formula are further calculated
(2) shown in:
Wherein, c represents the mean value of N number of gyroscope measurement vector mould in window, if var is greater than the second threshold of setting
thv, then determine that pedestrian is in nonstatic state at current time, otherwise, it is determined that pedestrian remains static at current time.
In one embodiment, step S3 is specifically included:
According to traditional inertial navigation algorithm, recursion obtains current posture, and strapdown posture renewal equation is as follows:
Wherein, w (t) is angular velocity of rotation of the Inertial Measurement Unit relative to navigational coordinate system under sensor coordinate system, qk+1
Indicate tk+1Four element of posture at moment, qk+1Indicate tkFour element of posture at moment.
In one embodiment, step S4 is specifically included:
Step S4.1: it establishes three axis accelerometer under current sensor coordinate system and really measures between vector and Attitude estimation
Relationship,
Wherein,Indicate the quaternary number form formula that local gravity projects under navigational coordinate system, qk+1For time tk+1Moment appearance
Four element of state,For time tk+1The quaternary number form of the true measurement vector of three axis accelerometer under moment sensor coordinate system
Formula；⊙ is quaternary number multiplication operator；
Step S4.2: constructing dynamic model with strapdown posture renewal equation, measurement model is constructed with formula (4), using karr
Graceful filter estimates current posture information.
In one embodiment, step S4.2 is specifically included:
Step S4.2.1: it using posture as state variable, is built according to the strapdown posture renewal equation in formula (4) and step S3
Discrete four elements Kalman filter model under Liru,
In formula (5), Fk+1Indicate state-transition matrix, Hk+1Indicate state observation matrix, Kk+1For system noise factor square
Battle array, Wk+1For observation noise coefficient matrix, δk+1Indicate the noise of the angle increment in the IMU sampling interval, εk+1Indicate 3-axis acceleration
Meter measurement noise, is defined as follows:
In formula (6), Δ θk+1It indicates from time tkTo tk+1The angle increment vector that three-axis gyroscope measures, Δ θk+1For vector
Δθk+1Mould, ImIndicate the unit matrix of m × m；gnIt is projection of the local gravity under navigational coordinate system；For time tk+1
The output vector of three axis accelerometer under moment carrier coordinate system；sk+1And vk+1It is four element q of posture respectivelyk+1Scalar component
And vector section.
Step S4.2.2: according to discrete four elements Kalman filter model, Attitude estimation after obtaining current time correction,
Wherein, state vector X in modelk+1=qk+1, in filtering, noise δk+1Association square matrix Qk+1With noise εk+1Covariance
Matrix Rk+1It is calculated as follows:
In formula (7), σ1Represent the standard deviation for the angle increment error that three-axis gyroscope in the IMU sampling interval measures, σ2Represent three
Axis accelerometer measures the standard deviation of noise, Mk|kAnd Mk+1|kFor intermediate variable, it is calculated as follows:
In formula (8), qk|kFor Kalman filtering last time Attitude estimation, qk+1|kFor the attitude prediction of "current" model, Pk|kWith
Pk+1/kRespectively qk|kAnd qk+1|kCovariance matrix.
In one embodiment, the method also includes:
Error compensation is carried out to three axis accelerometer.
Said one or multiple technical solutions in the embodiment of the present application at least have following one or more technology effects
Fruit:
In method provided by the invention, firstly, Inertial Measurement Unit is installed on pedestrian's foot, inertia measurement is initialized
Then initial attitude of the unit under navigational coordinate system judges whether pedestrian is in static shape according to the output of inertial sensor
State, when pedestrian is in nonstatic state, according to the measurement of initial attitude and three-axis gyroscope, using inertial navigation algorithm into
Row posture renewal, recursion obtain current posture, when pedestrian remains static, according to inertial navigation algorithm posture renewal side
Journey constructs dynamic model, accelerometer output and constructs measurement model with gravity relationship, is filtered under quaternary number frame by Kalman
Wave device is updated current posture.
The present invention utilizes the measurement construct Attitude estimation observed quantity of Still time accelerometer, executes under four element frames
Kalman filter carries out data fusion and obtains accurate posture information, to provide a kind of independent of external attitude sensing
Device, simple possible pedestrian navigation Attitude estimation method.It is accurate in the presence of being difficult to provide to solve method in the prior art
The technical issues of posture information.
Detailed description of the invention
In order to more clearly explain the embodiment of the invention or the technical proposal in the existing technology, to embodiment or will show below
There is attached drawing needed in technical description to be briefly described, it should be apparent that, the accompanying drawings in the following description is this hair
Bright some embodiments for those of ordinary skill in the art without creative efforts, can be with root
Other attached drawings are obtained according to these attached drawings.
Fig. 1 is a kind of indoor pedestrian navigation Attitude estimation method that formula Inertial Measurement Unit is worn based on foot provided by the invention
Flow chart；
Fig. 2 is the indoor pedestrian navigation Attitude estimation for wearing formula Inertial Measurement Unit in a kind of specific example of the present invention based on foot
The flow chart of method；
Fig. 3 is four element Kalman filtering flow charts in the embodiment of the present invention.
Specific embodiment
The embodiment of the invention provides a kind of indoor pedestrian navigation Attitude estimation sides that formula Inertial Measurement Unit is worn based on foot
Method, to improve method in the prior art there is technical issues that be difficult to provide it is accurate.
Technical solution in the embodiment of the present application, general thought are as follows:
A kind of indoor pedestrian navigation Attitude estimation method is provided, this method is using under stationary state in pedestrian's walking process
Accelerometer output construction attitude observation, under four element frames, by Kalman filter to inertial navigation algorithm posture
Estimation is corrected, it can be achieved that pedestrian's posture is effectively estimated under any navigational coordinate system.
In order to make the object, technical scheme and advantages of the embodiment of the invention clearer, below in conjunction with the embodiment of the present invention
In attached drawing, technical scheme in the embodiment of the invention is clearly and completely described, it is clear that described embodiment is
A part of the embodiment of the present invention, instead of all the embodiments.Based on the embodiments of the present invention, those of ordinary skill in the art
Every other embodiment obtained without creative efforts, shall fall within the protection scope of the present invention.
A kind of indoor pedestrian navigation Attitude estimation method for wearing formula Inertial Measurement Unit based on foot is present embodiments provided, is asked
Referring to Fig. 1, this method comprises:
Step S1: Inertial Measurement Unit is installed on pedestrian's foot, determines navigational coordinate system, to the zero of Inertial Measurement Unit
Inclined error carries out on-line proving, initializes initial attitude of the Inertial Measurement Unit under navigational coordinate system, wherein navigational coordinate system
Z axis it is perpendicular to the ground.
Specifically, navigational coordinate system is the frame that navigational parameter resolves.In outdoor, coordinate system is usually managed with the northeast world
As navigational coordinate system, in the present invention, using customized three-dimensional cartesian coordinate system as navigational coordinate system.Customized coordinate
On the one hand the selection of system avoids and uses terrestrial coordinate system as initial work complicated involved by navigational coordinate system, another
Aspect, which is also simplified, is transformed into required work under indoor map coordinate system for navigational parameter.The present invention has customized coordinate system
Following to require: customized coordinate system Z axis is perpendicular to the ground.In order to save the transformation between navigational parameter and map reference,
The 0XY plane of customized three-dimensional cartesian coordinate system can be overlapped with map coordinates system.
In one embodiment, step S1 is specifically included:
Step S1.1: Inertial Measurement Unit is installed on pedestrian's foot, customized three-dimensional cartesian coordinate system is sat as navigation
Mark system, it is ensured that the Z axis of navigational coordinate system is perpendicular to the ground；
Step S1.2: for three-axis gyroscope, with each average value axially exported in lower section of preset time of stationary state
Zero offset error as the axis.For three axis accelerometer, pass through the lower four different locations the preceding paragraph preset time of stationary state
Interior acceleration output is demarcated, shown in calibration formula such as formula (1),
Wherein, bx,by,bzRespectively zero offset error in three axis accelerometer X, Y, Z axis, xi、yi、zi, i=1,2,3,4, point
The average value that X, Y, Z are exported on i-th of position, m are not indicatedi, i=1,2,3,4 indicate that each axis exports average value on i-th of position
Quadratic sum；
Step S1.3: at the initial navigation moment, adjusting the position of Inertial Measurement Unit, sits Inertial Measurement Unit sensor
Mark system is aligned with navigational coordinate system, to obtain initial attitude of the Inertial Measurement Unit under navigational coordinate system, wherein initial appearance
The form of state is q0=[1,0,0,0]T。
Specifically, in pedestrian navigation, used Inertial Measurement Unit is usually inexpensive inertia device, even if
It has been demarcated before factory, but since the zero offset error stability of three axis accelerometer and three-axis gyroscope is poor, poor repeatability,
It needs to carry out on-line proving again to this error before every use, to improve the measurement accuracy of sensor.Three-axis gyroscope
Online calibration method is as follows: zero offset error of each average value axially exported as the axis in preset time using under stationary state.
It, can be by the way that under stationary state, four be since there are the influences of gravity to be demarcated by the above process for accelerometer
Output with accelerometer in preset time on position is demarcated.Wherein, preset time can according to the actual situation and experience
It is selected.
After navigational coordinate system determines, initial attitude of the Inertial Measurement Unit under navigational coordinate system can be simple as follows
It is single to determine: at the initial navigation moment, to adjust the position of Inertial Measurement Unit.The present invention makes Inertial Measurement Unit sensor coordinate system
It is aligned with navigational coordinate system, so that initial attitude is expressed as q with four element forms0=[1,0,0,0]T。
Step S2: judging whether pedestrian remains static according to the output of three-axis gyroscope, if pedestrian is in non-quiet
Only state executes step S3, no to then follow the steps S4.
In one embodiment, step S2 is specifically included:
Step S2.1: judging whether the sequence total length of current Inertial Measurement Unit output is greater than or equal to preset length N,
If it is, going to step S2.2, judge whether pedestrian remains static；
Step S2.2: current three-axis gyroscope measurement vector field homoemorphism is calculated | | yk- b | |, wherein ykFor current three axis accelerometer
Instrument measures vector, and b is the zero bias vector of three-axis gyroscope obtained in step S1, and judges | | yk- b | | whether it is greater than setting
First threshold thm, if it is greater, then determining that pedestrian is in nonstatic state at current time, otherwise, execute step S2.3；
Step S2.3: the variance var of the gyroscope measurement vector mould in fixed length window, calculation formula such as formula are further calculated
(2) shown in:
Wherein, c represents the mean value of N number of gyroscope measurement vector mould in window, if var is greater than the second threshold of setting
thv, then determine that pedestrian is in nonstatic state at current time, otherwise, it is determined that pedestrian remains static at current time.
Specifically, Fig. 2 is referred to, is the flow chart of a kind of indoor pedestrian navigation Attitude estimation method in specific example.
The present invention exports detecting pedestrian stationary state according to inertial sensor, and the present embodiment uses current three-axis gyroscope output valve amplitude
The detection of pedestrian's stationary state is carried out with gyroscope output valve amplitude variance in fixed length window, it is assumed that length of window N.If current
Inertial sensor output sequence total length is less than N, can not carry out static detection, then goes to step S3 execution.Otherwise, pass through step
S2.2 and step S2.3 is judged.
In step s 2, first threshold thmWith second threshold thvIt selects as follows as follows: being mark according in step S1
The static gyro data for determining the acquisition of three-axis gyroscope zero offset error calculates all gyroscope measurement vector field homoemorphisms in this group of data,
And maximum modulus value is found, using its 1.5 times as thm；Since n-th data, calculate each gyroscope measurement vector modulus value with
The variance of data set composed by preceding N-1 gyroscope measurement vector modulus value, finds variance maximum value, using its 1.5 times as
thv。
In step s 2, the setting of length of window N can be according to during one during pedestrian's normal walking complete gait
Static duration determines.It is quiet in a complete gait during many experiments test result of different researchers shows pedestrian's normal walking
Only duration is about 0.3-0.5 seconds.Therefore, N may be configured as 0.3F, and F indicates the output frequency of inertial sensor.Present inventor
Found by largely practicing with research: N cannot be arranged too greatly, will lead to very much true stationary state greatly and be missed；N is also not easy to set
Set it is too small, it is too small to detect multiple stationary states in a gait, cause computing overhead to increase.
Certainly, in specific implementation, other modes can also be used to realize in the detection of stationary state, and the present invention does not make this
Especially limitation.
Step S3: according to the measurement of initial attitude and three-axis gyroscope, carrying out posture renewal using inertial navigation algorithm,
Recursion obtains current posture, wherein the influence for not considering earth rotation, using the measurement of three-axis gyroscope as sensor coordinates
It is lower angular velocity of rotation of the Inertial Measurement Unit relative to navigational coordinate system.
Specifically, according to traditional inertial navigation algorithm, recursion obtains current posture.In recursive algorithm, need to survey
Angular velocity of rotation of the Inertial Measurement Unit relative to navigational coordinate system under quantity sensor coordinate system, and three in Inertial Measurement Unit
The output of axis gyroscope is measured as angular velocity of rotation of the Inertial Measurement Unit relative to navigational coordinate system under sensor coordinate system, because
This needs therefrom to deduct the influence of earth rotation.Under customized navigational coordinate system, to three-axis gyroscope measurement gyroscope output
It is extremely difficult to carry out rotational-angular velocity of the earth compensation.But rotational-angular velocity of the earth is very small, and about 0.004 angle is per second, therefore
Can be ignored, in the present invention, the measurement of three-axis gyroscope be under sensor coordinate system Inertial Measurement Unit relative to leading
The angular velocity of rotation of boat coordinate system.
In one embodiment, step S3 is specifically included:
According to traditional inertial navigation algorithm, recursion obtains current posture, and strapdown posture renewal equation is as follows:
Wherein, w (t) is angular velocity of rotation of the Inertial Measurement Unit relative to navigational coordinate system under sensor coordinate system, qk+1
Indicate tk+1Four element of posture at moment, qkIndicate tkFour element of posture at moment.
Specifically, after recursion posture renewal is finished, judge whether navigation terminates, if it is, terminating posture more
Newly, step S2 is otherwise returned to.
Step S4: dynamic model, accelerometer output are constructed according to inertial navigation algorithm posture renewal equation and closed with gravity
Series structure measurement model is updated current posture by Kalman filter under quaternary number frame.
Specifically, posture renewal is carried out by Kalman filter under quaternary number frame.According to inertial navigation algorithm
The posture that can push away currently, but due to sensor error, there are biggish errors for posture.The present invention utilizes the static shape of pedestrian
Three axis accelerometer construction attitude observation under state corrects current pose.
Wherein, step S4 is specifically included:
Step S4.1: establish current three axis accelerometer under sensor coordinate system really measure vector (do not consider zero bias and
Other errors) and Attitude estimation between relationship,
Wherein,Indicate the quaternary number form formula that local gravity projects under navigational coordinate system, qk+1For time tk+1Moment appearance
Four element of state,For time tk+1The quaternary number form of the true measurement vector of three axis accelerometer under moment sensor coordinate system
Formula；⊙ is quaternary number multiplication operator；
Step S4.2: constructing dynamic model with strapdown posture renewal equation, measurement model is constructed with formula (4), using karr
Graceful filter estimates current posture information.
Specifically, Fig. 3 is referred to, is four element Kalman filtering flow charts.It can be passed according to the formula in step S3
It pushes away to obtain current pose.But but due to sensor error, the obtained attitude estimation error is larger, need using can
The external observation amount leaned on is corrected.Under pedestrian's stationary state, the measurement of three axis accelerometer only reflects terrestrial gravitation shadow
Ring (namely measured value be equal to projection of the gravity under current coordinate system), and projection of the gravity under navigational coordinate system it is known that
Then the relationship between current three axis accelerometer measurement and Attitude estimation can be established.
For the quaternary number form formula that local gravity projects under navigational coordinate system, according to the definition of navigational coordinate system, gravity
Projection under navigational coordinate system is known.Three-dimensional vector can switch to quaternary number form formula by supplementing 0 element, such as assume
Navigational coordinate system is upward perpendicular to the ground, and gravity is projected as g under navigational coordinate systemn=[0 0-g]T, it is rewritten into quaternary number form formula
ForIn indoor navigation application, the Gravity changer in navigation area is not considered, and gravity g is constant.
In one embodiment, step S4.2 is specifically included:
Step S4.2.1: it using posture as state variable, is built according to the strapdown posture renewal equation in formula (4) and step S3
Discrete four elements Kalman filter model under Liru,
In formula (5), Fk+1Indicate state-transition matrix, Hk+1Indicate state observation matrix, Kk+1For system noise factor square
Battle array, Wk+1For observation noise coefficient matrix, δk+1Indicate the noise of the angle increment in the IMU sampling interval, εk+1Indicate 3-axis acceleration
Meter measurement noise, is defined as follows:
In formula (6), Δ θk+1It indicates from time tkTo tk+1The angle increment vector that three-axis gyroscope measures, Δ θk+1For vector
Δθk+1Mould, ImIndicate the unit matrix of m × m；gnIt is projection of the local gravity under navigational coordinate system；For time tk+1When
Carve the output vector of three axis accelerometer under carrier coordinate system；sk+1And vk+1It is four element q of posture respectivelyk+1Scalar component and
Vector section.
Step S4.2.2: according to discrete four elements Kalman filter model, Attitude estimation after obtaining current time correction,
Wherein, state vector X in modelk+1=qk+1, in filtering, noise δk+1Association square matrix Qk+1With noise εk+1Covariance
Matrix Rk+1It is calculated as follows:
In formula (7), σ1Represent the standard deviation for the angle increment error that three-axis gyroscope in the IMU sampling interval measures, σ2Represent three
Axis accelerometer measures the standard deviation of noise, Mk|kAnd Mk+1|kFor intermediate variable, it is calculated as follows:
In formula (8), qk|kFor Kalman filtering last time Attitude estimation, qk+1|kFor the attitude prediction of "current" model, Pk|kWith
Pk+1/kRespectively qk|kAnd qk+1|kCovariance matrix.
Specifically, system model is non-linearization equation in the filter, to use traditional Kalman filter into
Row data fusion is extremely complex, therefore the present invention executes Kalman filter under four element frames.Using four element of posture as shape
State amount establishes discrete four elements Kalman filter model.After filtering is finished, step S2 is come back to.
Kalman filter is executed in the present invention under four element frames compared with common Kalman filter, difference
Be: the two executes under different frames, and one is common three-dimensional vector frame, and one is difference under four element frames
Algorithm under frame is different, four element Kalman filterings it is possible to prevente effectively from traditional Kalman filtering complex linear
Problem.
In order to obtain accurately and reliably 3-axis acceleration measurement, in one embodiment, the method also includes: to three
Axis accelerometer carries out error compensation.
Specifically, the present invention considers three axis accelerometer error and changes with time.It, can be in specific implementation process
Three axis accelerometer error compensation is carried out using any feasible method, this discovery is not particularly limited this.In the present embodiment,
Following compensation method is provided.Three axis accelerometer items error is influenced into unification into zero offset error, it is random to establish zero offset error
Migration model,
bk+1=bk+ηk+1 (9)
In formula (9), bk+1And bkRepresent the zero offset error at current time and last moment three axis accelerometer, ηk+1It is current
Noise.Using Zero velocity Updating method, using the speed under pedestrian's stationary state as observed quantity, pedestrian's speed and three axis accelerometer
Deviation can establish the zero bias size of following Kalman Filtering for Discrete model estimation three axis accelerometer as quantity of state,
In formula (10), xkThe state vector being made of pedestrian's present speed and three axis accelerometer deviation, Ak+1It is state
Transfer matrix, Bk+1It is state observation matrix, ωk+1WithRespectively system noise and observation noise.Ak+1And Bk+1Definition is such as
Under,
In formula, T is inertial sensor unit output time interval,Indicate sensor coordinate system to navigational coordinate system
Direction cosine matrix, I3×1With 03×1Respectively represent element be all 1 three dimensional vectors and element be all 0 three dimensional vectors.
Four element Kalman filterings are finished, and judge whether navigation terminates, and then terminate posture renewal in this way, such as otherwise return to step
S2。
Said one or multiple technical solutions in the embodiment of the present application at least have following one or more technology effects
Fruit:
The present invention utilizes the measurement construct Attitude estimation observed quantity of Still time accelerometer, executes under four element frames
Kalman filter carries out data fusion and obtains accurate posture information, to provide a kind of independent of external attitude sensing
Device, simple possible pedestrian navigation Attitude estimation method.
Although preferred embodiments of the present invention have been described, it is created once a person skilled in the art knows basic
Property concept, then additional changes and modifications may be made to these embodiments.So it includes excellent that the following claims are intended to be interpreted as
It selects embodiment and falls into all change and modification of the scope of the invention.
Obviously, those skilled in the art can carry out various modification and variations without departing from this hair to the embodiment of the present invention
The spirit and scope of bright embodiment.In this way, if these modifications and variations of the embodiment of the present invention belong to the claims in the present invention
And its within the scope of equivalent technologies, then the present invention is also intended to include these modifications and variations.
Claims (7)
1. wearing the indoor pedestrian navigation Attitude estimation method of formula Inertial Measurement Unit based on foot characterized by comprising
Step S1: being installed on pedestrian's foot for Inertial Measurement Unit, determine navigational coordinate system, misses to the zero bias of Inertial Measurement Unit
Difference carries out on-line proving, initializes initial attitude of the Inertial Measurement Unit under navigational coordinate system, wherein the Z of navigational coordinate system
Axis is perpendicular to the ground；
Step S2: judging whether pedestrian remains static according to the output of inertial sensor, if pedestrian is in nonstatic shape
State executes step S3, no to then follow the steps S4；
Step S3: according to the output of initial attitude and three-axis gyroscope, posture renewal, recursion are carried out using inertial navigation algorithm
Obtain current posture, wherein using the measurement of three-axis gyroscope as Inertial Measurement Unit under sensor coordinate system relative to leading
The angular velocity of rotation of boat coordinate system；
Step S4: dynamic model, accelerometer output and gravity relationship structure are constructed according to inertial navigation algorithm posture renewal equation
Measurement model is made, current posture is updated by Kalman filter under quaternary number frame.
2. the method as described in claim 1, which is characterized in that step S1 is specifically included:
Step S1.1: being installed on pedestrian's foot for Inertial Measurement Unit, customized three-dimensional cartesian coordinate system as navigational coordinate system,
Ensure that the Z axis of navigational coordinate system is perpendicular to the ground；
Step S1.2: for three-axis gyroscope, using each average value axially exported in lower section of preset time of stationary state as
The zero offset error of the axis；For three axis accelerometer, by the lower four different locations the preceding paragraph preset time of stationary state
Acceleration output is demarcated, shown in calibration formula such as formula (1),
Wherein, bx,by,bzRespectively zero offset error in three axis accelerometer X, Y, Z axis, xi、yi、zi, i=1,2,3,4, difference table
Show the average value that X, Y, Z are exported on i-th of position, mi, i=1,2,3,4 indicate that each axis exports the flat of average value on i-th of position
Fang He；
Step S1.3: at the initial navigation moment, the position of Inertial Measurement Unit is adjusted, makes Inertial Measurement Unit sensor coordinate system
It is aligned with navigational coordinate system, to obtain initial attitude of the Inertial Measurement Unit under navigational coordinate system, wherein initial attitude
Form is q0=[1,0,0,0]T。
3. the method as described in claim 1, which is characterized in that step S2 is specifically included:
Step S2.1: judging whether the sequence total length of current Inertial Measurement Unit output is greater than or equal to preset length N, if
It is then to go to step S2.2, judges whether pedestrian remains static；
Step S2.2: current three-axis gyroscope measurement vector field homoemorphism is calculated | | yk- b | |, wherein ykFor the survey of current three-axis gyroscope
Vector is measured, b is the zero bias vector of three-axis gyroscope obtained in step S1, and judges | | yk- b | | whether it is greater than the first of setting
Threshold value thm, if it is greater, then determining that pedestrian is in nonstatic state at current time, otherwise, execute step S2.3；
Step S2.3: the variance var of the gyroscope measurement vector mould in fixed length window, calculation formula such as formula (2) are further calculated
It is shown:
Wherein, c represents the mean value of N number of gyroscope measurement vector mould in window, if var is greater than the second threshold th of settingv, then
Determine that pedestrian is in nonstatic state at current time, otherwise, it is determined that pedestrian remains static at current time.
4. the method as described in claim 1, which is characterized in that step S3 is specifically included:
According to traditional inertial navigation algorithm, recursion obtains current posture, and strapdown posture renewal equation is as follows:
Wherein, w (t) is angular velocity of rotation of the Inertial Measurement Unit relative to navigational coordinate system under sensor coordinate system, qk+1It indicates
tk+1Four element of posture at moment, qkIndicate tkFour element of posture at moment.
5. the method as described in claim 1, which is characterized in that step S4 is specifically included:
Step S4.1: establishing acceleration under current sensor coordinate system and really measure relationship between vector and Attitude estimation,
Wherein,Indicate the quaternary number form formula that local gravity projects under navigational coordinate system, qk+1For time tk+1Moment posture four
Element,For time tk+1The quaternary number form formula of the true measurement vector of three axis accelerometer under moment sensor coordinate system；⊙
For quaternary number multiplication operator；
Step S4.2: constructing dynamic model with strapdown posture renewal equation, constructs measurement model with formula (4), is filtered using Kalman
Wave device estimates current posture information.
6. method as claimed in claim 5, which is characterized in that step S4.2 is specifically included:
Step S4.2.1: using posture as state variable, such as according to the strapdown posture renewal establishing equation in formula (4) and step S3
Under discrete four elements Kalman filter model,
In formula (5), Fk+1Indicate state-transition matrix, Hk+1Indicate state observation matrix, Kk+1For system noise factor matrix, Wk+1
For observation noise coefficient matrix, δk+1Indicate the noise of the angle increment in the IMU sampling interval, εk+1Indicate three axis accelerometer measurement
Noise is defined as follows:
In formula (6), Δ θk+1It indicates from time tkTo tk+1The angle increment vector that three-axis gyroscope measures, Δ θk+1For vector Δ θk+1
Mould, ImIndicate the unit matrix of m × m；gnIt is projection of the local gravity under navigational coordinate system；For time tk+1Moment carries
The output vector of three axis accelerometer under body coordinate system；sk+1And vk+1It is four element q of posture respectivelyk+1Scalar component and vector
Part；
Step S4.2.2: according to discrete four elements Kalman filter model, Attitude estimation after obtaining current time correction,
In, state vector X in modelk+1=qk+1, in filtering, noise δk+1Association square matrix Qk+1With noise εk+1Covariance square
Battle array Rk+1It is calculated as follows:
In formula (7), σ1Represent the standard deviation for the angle increment error that three-axis gyroscope in the Inertial Measurement Unit sampling interval measures, σ2
Represent the standard deviation of three axis accelerometer measurement noise, Mk|kAnd Mk+1|kFor intermediate variable, it is calculated as follows:
In formula (8), qk|kFor Kalman filtering last time Attitude estimation, qk+1|kFor the attitude prediction of "current" model, Pk|kAnd Pk+1/k
Respectively qk|kAnd qk+1|kCovariance matrix.
7. the method as described in any one of claim 1-6 claim, which is characterized in that the method also includes:
Error compensation is carried out to three axis accelerometer.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910615908.6A CN110398245B (en) | 2019-07-09 | 2019-07-09 | Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910615908.6A CN110398245B (en) | 2019-07-09 | 2019-07-09 | Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110398245A true CN110398245A (en) | 2019-11-01 |
| CN110398245B CN110398245B (en) | 2021-04-16 |
Family
ID=68324071
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910615908.6A Active CN110398245B (en) | 2019-07-09 | 2019-07-09 | Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110398245B (en) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110887481A (en) * | 2019-12-11 | 2020-03-17 | 中国空气动力研究与发展中心低速空气动力研究所 | Carrier dynamic attitude estimation method based on MEMS inertial sensor |
| CN110916577A (en) * | 2019-12-17 | 2020-03-27 | 小狗电器互联网科技（北京）股份有限公司 | Robot static state judgment method and device and robot |
| CN111366154A (en) * | 2020-03-26 | 2020-07-03 | 湖南三一快而居住宅工业有限公司 | Course angle determining method and device and electronic equipment |
| CN111721282A (en) * | 2020-05-09 | 2020-09-29 | 中国人民解放军63686部队 | Strapdown inertial navigation coordinate system dynamic alignment method based on astronomical navigation principle |
| CN111780785A (en) * | 2020-07-20 | 2020-10-16 | 武汉中海庭数据技术有限公司 | Zero offset self-calibration method and system for vehicle-mounted MEMSIMU |
| CN112082529A (en) * | 2020-07-29 | 2020-12-15 | 上海谷感智能科技有限公司 | Small household appliance attitude measurement method based on inertial sensor and attitude identification module |
| CN112781622A (en) * | 2020-12-31 | 2021-05-11 | 厦门华源嘉航科技有限公司 | Pedestrian navigation MIMU installation error online calibration method |
| CN112945240A (en) * | 2021-03-16 | 2021-06-11 | 北京三快在线科技有限公司 | Method, device and equipment for determining positions of feature points and readable storage medium |
| CN113008230A (en) * | 2021-02-26 | 2021-06-22 | 广州偶游网络科技有限公司 | Intelligent wearable device and posture orientation recognition method and device thereof |
| CN114018250A (en) * | 2021-10-18 | 2022-02-08 | 杭州鸿泉物联网技术股份有限公司 | Inertial navigation method, electronic device, storage medium, and computer program product |
| CN114563018A (en) * | 2022-03-04 | 2022-05-31 | 闪耀现实(无锡)科技有限公司 | Method and apparatus for calibrating head mounted display device |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102927994A (en) * | 2012-10-23 | 2013-02-13 | 北京航空航天大学 | Method of quickly calibrating oblique redundant strapdown inertial navigation system |
| CN103616030A (en) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | Autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction |
| US20140324291A1 (en) * | 2003-03-20 | 2014-10-30 | Agjunction Llc | Gnss and optical guidance and machine control |
| RU2539140C1 (en) * | 2013-08-02 | 2015-01-10 | Олег Степанович Салычев | Integrated strapdown system of navigation of average accuracy for unmanned aerial vehicle |
| CN106705968A (en) * | 2016-12-09 | 2017-05-24 | 北京工业大学 | Indoor inertial navigation algorithm based on posture recognition and step length model |
| CN106949889A (en) * | 2017-03-17 | 2017-07-14 | 南京航空航天大学 | For the inexpensive MEMS/GPS integrated navigation systems and method of pedestrian navigation |
| CN109163721A (en) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | Attitude measurement method and terminal device |
-
2019
- 2019-07-09 CN CN201910615908.6A patent/CN110398245B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140324291A1 (en) * | 2003-03-20 | 2014-10-30 | Agjunction Llc | Gnss and optical guidance and machine control |
| CN102927994A (en) * | 2012-10-23 | 2013-02-13 | 北京航空航天大学 | Method of quickly calibrating oblique redundant strapdown inertial navigation system |
| RU2539140C1 (en) * | 2013-08-02 | 2015-01-10 | Олег Степанович Салычев | Integrated strapdown system of navigation of average accuracy for unmanned aerial vehicle |
| CN103616030A (en) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | Autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction |
| CN106705968A (en) * | 2016-12-09 | 2017-05-24 | 北京工业大学 | Indoor inertial navigation algorithm based on posture recognition and step length model |
| CN106949889A (en) * | 2017-03-17 | 2017-07-14 | 南京航空航天大学 | For the inexpensive MEMS/GPS integrated navigation systems and method of pedestrian navigation |
| CN109163721A (en) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | Attitude measurement method and terminal device |
Non-Patent Citations (1)
| Title |
|---|
| 肖业伦: "《航空航天器运动的建模——飞行动力学的理论基础》", 30 June 2003, 北京航空航天大学出版社 * |
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110887481A (en) * | 2019-12-11 | 2020-03-17 | 中国空气动力研究与发展中心低速空气动力研究所 | Carrier dynamic attitude estimation method based on MEMS inertial sensor |
| CN110916577A (en) * | 2019-12-17 | 2020-03-27 | 小狗电器互联网科技（北京）股份有限公司 | Robot static state judgment method and device and robot |
| CN111366154A (en) * | 2020-03-26 | 2020-07-03 | 湖南三一快而居住宅工业有限公司 | Course angle determining method and device and electronic equipment |
| CN111366154B (en) * | 2020-03-26 | 2022-05-17 | 三一建筑机器人(西安)研究院有限公司 | Course angle determining method and device and electronic equipment |
| CN111721282B (en) * | 2020-05-09 | 2022-05-03 | 中国人民解放军63686部队 | Strapdown inertial navigation coordinate system dynamic alignment method based on astronomical navigation principle |
| CN111721282A (en) * | 2020-05-09 | 2020-09-29 | 中国人民解放军63686部队 | Strapdown inertial navigation coordinate system dynamic alignment method based on astronomical navigation principle |
| CN111780785A (en) * | 2020-07-20 | 2020-10-16 | 武汉中海庭数据技术有限公司 | Zero offset self-calibration method and system for vehicle-mounted MEMSIMU |
| CN112082529A (en) * | 2020-07-29 | 2020-12-15 | 上海谷感智能科技有限公司 | Small household appliance attitude measurement method based on inertial sensor and attitude identification module |
| CN112781622A (en) * | 2020-12-31 | 2021-05-11 | 厦门华源嘉航科技有限公司 | Pedestrian navigation MIMU installation error online calibration method |
| CN112781622B (en) * | 2020-12-31 | 2022-07-05 | 厦门华源嘉航科技有限公司 | Pedestrian navigation MIMU installation error online calibration method |
| CN113008230A (en) * | 2021-02-26 | 2021-06-22 | 广州偶游网络科技有限公司 | Intelligent wearable device and posture orientation recognition method and device thereof |
| CN113008230B (en) * | 2021-02-26 | 2024-04-02 | 广州市偶家科技有限公司 | Intelligent wearable device and gesture direction recognition method and device thereof |
| CN112945240A (en) * | 2021-03-16 | 2021-06-11 | 北京三快在线科技有限公司 | Method, device and equipment for determining positions of feature points and readable storage medium |
| CN114018250A (en) * | 2021-10-18 | 2022-02-08 | 杭州鸿泉物联网技术股份有限公司 | Inertial navigation method, electronic device, storage medium, and computer program product |
| CN114018250B (en) * | 2021-10-18 | 2024-05-03 | 杭州鸿泉物联网技术股份有限公司 | Inertial navigation method, electronic device, storage medium and computer program product |
| CN114563018A (en) * | 2022-03-04 | 2022-05-31 | 闪耀现实(无锡)科技有限公司 | Method and apparatus for calibrating head mounted display device |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110398245B (en) | 2021-04-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110398245A (en) | The indoor pedestrian navigation Attitude estimation method of formula Inertial Measurement Unit is worn based on foot | |
| CN107289933B (en) | Double card Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion | |
| CN109991636A (en) | Map constructing method and system based on GPS, IMU and binocular vision | |
| CN105043385B (en) | A kind of method for adaptive kalman filtering of pedestrian's Camera calibration | |
| Huang et al. | Synergism of INS and PDR in self-contained pedestrian tracking with a miniature sensor module | |
| CN106500693B (en) | A kind of AHRS algorithm based on adaptive extended kalman filtering | |
| US8010308B1 (en) | Inertial measurement system with self correction | |
| CN109540126A (en) | A kind of inertia visual combination air navigation aid based on optical flow method | |
| CN106979780B (en) | A kind of unmanned vehicle real-time attitude measurement method | |
| CN103776446B (en) | A kind of pedestrian's independent navigation computation based on double MEMS-IMU | |
| CN109099913B (en) | Wearable navigation device and method based on MEMS inertial device | |
| CN109163721A (en) | Attitude measurement method and terminal device | |
| CN103822633B (en) | A kind of low cost Attitude estimation method measuring renewal based on second order | |
| CN110398257A (en) | The quick initial alignment on moving base method of SINS system of GPS auxiliary | |
| CN101290229A (en) | Silicon micro-navigation attitude system inertia/geomagnetism assembled method | |
| CN107270898B (en) | Double particle filter navigation devices and method based on MEMS sensor and VLC positioning fusion | |
| CN110221332A (en) | A kind of the dynamic lever arm estimation error and compensation method of vehicle-mounted GNSS/INS integrated navigation | |
| CN109916395A (en) | A kind of autonomous Fault-tolerant Integrated navigation algorithm of posture | |
| CN109931955A (en) | Strapdown inertial navigation system Initial Alignment Method based on the filtering of state correlation Lie group | |
| CN104937377B (en) | Method and apparatus for the vertical orientation for handling unconfined portable navigating device | |
| CN109186597A (en) | A kind of localization method of the indoor wheeled robot based on double MEMS-IMU | |
| CN107246872B (en) | Single-particle filtering navigation device and method based on MEMS sensor and VLC positioning fusion | |
| CN110764506A (en) | Course angle fusion method and device of mobile robot and mobile robot | |
| CN110440746A (en) | A kind of no-dig technique subterranean drill bit posture fusion method based on the decline of quaternary number gradient | |
| CN107677267A (en) | Indoor pedestrian navigation course feedback modifiers method based on MEMS IMU |
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 |