CN110398245B - Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit - Google Patents

Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit Download PDF

Info

Publication number
CN110398245B
CN110398245B CN201910615908.6A CN201910615908A CN110398245B CN 110398245 B CN110398245 B CN 110398245B CN 201910615908 A CN201910615908 A CN 201910615908A CN 110398245 B CN110398245 B CN 110398245B
Authority
CN
China
Prior art keywords
attitude
coordinate system
navigation
measurement unit
pedestrian
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910615908.6A
Other languages
Chinese (zh)
Other versions
CN110398245A (en
Inventor
文铠
余科根
李英冰
梁伟
张帅
周力璇
金荣河
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN201910615908.6A priority Critical patent/CN110398245B/en
Publication of CN110398245A publication Critical patent/CN110398245A/en
Application granted granted Critical
Publication of CN110398245B publication Critical patent/CN110398245B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

Abstract

The invention provides an indoor pedestrian navigation attitude estimation method based on a foot-worn inertial measurement unit, which comprises the steps of firstly, installing the inertial measurement unit on the foot of a pedestrian, initializing the initial attitude of the inertial measurement unit under a navigation coordinate system, then judging whether the pedestrian is in a static state or not according to the output of an inertial sensor, when the pedestrian is in a non-static state, updating the attitude by adopting a strapdown inertial navigation algorithm, recursion to obtain the current attitude, when the pedestrian is in a static state, constructing a dynamic model according to an attitude update equation of the strapdown inertial navigation algorithm, constructing a measurement model according to the output of an accelerometer and the gravity relation, and updating the current attitude by a Kalman filter under a quaternion frame. According to the invention, the attitude estimation observed quantity is constructed by using the measurement of the accelerometer at the static moment, and the Kalman filter is executed to perform data fusion under the four-element framework to obtain accurate attitude information, so that the technical effect of providing accurate attitude information is realized.

Description

Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit
Technical Field
The invention relates to the technical field of pedestrian navigation, in particular to an indoor pedestrian navigation attitude estimation method based on a foot-worn inertial measurement unit.
Background
The "pedestrian navigation" is to provide necessary position and direction information on the basis of a map and guide a pedestrian to a predetermined target. In the abundant intelligent electronic devices, pedestrian navigation becomes an important function (such as pedestrian navigation services provided by high-definition maps and Baidu maps in smart phones), and the travel efficiency of people is greatly improved.
In the outdoor environment, accurate position information can be obtained by means of a Global Navigation Satellite System (GNSS), and accurate pedestrian Navigation can be realized by combining with other direction sensors (such as a gyroscope). However, in the indoor environment, due to the shielding of the building structure and the wall, GNSS signals are severely attenuated, and effective position information cannot be estimated. Therefore, indoor pedestrian navigation is more difficult and requires development of special technology, and is one of the hot spots of the current research in the field of home and abroad positioning navigation.
In the related art, available indoor location estimation techniques can be roughly classified into a vision-based method, a wireless signal-based method, an inertial sensor-based method, and the like. Considering the problems of cost, power consumption, computational complexity and the like, the method based on the Inertial Measurement Unit (IMU) is widely applied to indoor pedestrian navigation position estimation. How to acquire accurate direction information is a main difficulty of indoor pedestrian navigation. The direction estimation by means of the measurement of the IMU built-in angular velocity sensor is an economical and simple method, but has the problem of direction drift, and the direction estimation needs to be continuously corrected so as to limit the direction drift. Current solutions include zero acceleration update algorithms, magnetic heading assist algorithms, and the like.
The inventor of the present application finds that the method of the prior art has at least the following technical problems in the process of implementing the present invention:
the zero acceleration updating algorithm depends on the used attitude error model and is only effective under the terrestrial coordinate system; the magnetic heading auxiliary algorithm needs an accurate heading observation value, but because indoor geomagnetic interference is serious, a large error exists in heading estimation obtained through geomagnetic measurement. In addition, the solutions need to be performed in a terrestrial coordinate system, so that the initialization work of the navigation system is greatly increased; also, these solutions generally provide only heading information, and do not provide accurate three-dimensional directional information (i.e., attitude). For some special indoor pedestrian navigation applications, such as firefighter navigation, the posture information is beneficial to monitoring the walking state (standing, lying, squatting, etc.) of the navigated person so as to guess whether the person is safe or not.
Therefore, the method in the prior art has the technical problem that accurate attitude information is difficult to provide.
Disclosure of Invention
In order to solve the problem that the current indoor pedestrian navigation system is difficult to provide accurate attitude information, the invention provides an indoor pedestrian navigation attitude estimation method based on a foot-worn IMU.
The invention provides an indoor pedestrian navigation attitude estimation method based on a foot-worn IMU, which comprises the following steps:
step S1: the method comprises the steps of installing an inertia measurement unit on the foot of a pedestrian, determining a navigation coordinate system, carrying out online calibration on zero offset error of the inertia measurement unit, and initializing the initial posture of the inertia measurement unit under the navigation coordinate system, wherein the Z axis of the navigation coordinate system is vertical to the ground;
step S2: judging whether the pedestrian is in a static state according to the output of the inertial sensor, if so, executing step S3, otherwise, executing step S4;
step S3: according to the initial attitude and the output of the triaxial gyroscope, adopting a strapdown inertial navigation algorithm to update the attitude, and recursion to obtain the current attitude, wherein the measurement of the triaxial gyroscope is used as the rotation angular velocity of an inertial measurement unit relative to a navigation coordinate system under a sensor coordinate system;
step S4: and constructing a dynamic model according to a strapdown inertial navigation algorithm attitude updating equation, constructing a measurement model according to the relation between accelerometer output and gravity, and updating the current attitude through a Kalman filter under a quaternion frame.
In one embodiment, step S1 specifically includes:
step S1.1: the method comprises the following steps of (1) installing an inertia measurement unit on a foot of a pedestrian, and defining a three-dimensional rectangular coordinate system as a navigation coordinate system to ensure that a Z axis of the navigation coordinate system is vertical to the ground;
step S1.2: for the three-axis gyroscope, the average value of all axial outputs in a preset time period in a static state is used as the zero offset error of the axis. For a triaxial accelerometer, the output of the accelerometer in a preset time period at four different positions in a static state is calibrated, a calibration formula is shown as a formula (1),
Figure BDA0002123934930000031
wherein, bx,by,bzRespectively, the on-axis zero offset error, x, of the three-axis accelerometer X, Y, Zi、yi、ziI is 1,2,3,4, and represents the average value of X, Y, Z outputs at the i-th position, respectively, and m isiI is 1,2,3,4, which represents the sum of the squares of the average output values of the axes at the i-th position;
step S1.3: at the initial navigation time, adjusting the position of the inertial measurement unit to align the sensor coordinate system of the inertial measurement unit with the navigation coordinate system, thereby obtaining the initial attitude of the inertial measurement unit under the navigation coordinate system, wherein the initial attitude is in the form of q0＝[1,0,0,0]T。
In one embodiment, step S2 specifically includes:
step S2.1: judging whether the total length of the sequence output by the current inertia measurement unit is greater than or equal to a preset length N, if so, turning to the step S2.2, and judging whether the pedestrian is in a static state;
step S2.2: calculating the modulo y of the current triaxial gyroscope measurement vectork-b | |, wherein ykB is the zero-offset vector of the three-axis gyroscope obtained in step S1, and determines yk-whether b | | is greater than a set first threshold value thmIf the pedestrian is in the non-stationary state, judging that the pedestrian is in the non-stationary state at the current moment, otherwise, executing the step S2.3;
step S2.3: further calculating the variance var of the gyroscope measurement vector mode in the fixed-length window, wherein the calculation formula is shown as formula (2):
Figure BDA0002123934930000032
wherein c represents the mean value of the N gyroscope measurement vector moduli in the window, and if var is greater than a set second threshold thvAnd if not, determining that the pedestrian is in the static state at the current moment.
In one embodiment, step S3 specifically includes:
and (3) recursion is carried out according to a traditional strapdown inertial navigation algorithm to obtain the current posture, and the strapdown posture updating equation is as follows:
Figure BDA0002123934930000033
wherein w (t) is the rotation angular velocity of the inertial measurement unit relative to the navigation coordinate system under the sensor coordinate system, qk+1Represents tk+1Attitude four elements of time, qk+1Represents tkAnd four elements of the posture of the moment.
In one embodiment, step S4 specifically includes:
step S4.1: establishing the relation between the true measurement vector of the triaxial accelerometer and the attitude estimation in the current sensor coordinate system,
Figure BDA0002123934930000041
wherein the content of the first and second substances,
Figure BDA0002123934930000042
quaternion form, q, representing the projection of local gravity under the navigation coordinate systemk+1Is a time tk+1The posture of the time is four elements,
Figure BDA0002123934930000043
is a time tk+1A quaternion form of a real measurement vector of the triaxial accelerometer under a time sensor coordinate system; an as quaternion multiplication operator;
step S4.2: and (3) constructing a dynamic model by using a strapdown attitude updating equation, constructing a measurement model by using an equation (4), and estimating the current attitude information by using a Kalman filter.
In one embodiment, step S4.2 specifically includes:
step S4.2.1: with the attitude as the state variable, the following discrete four-element Kalman filtering model is established according to the equation (4) and the strapdown attitude update equation in the step S3,
Figure BDA0002123934930000044
in the formula (5), Fk+1Represents the state transition matrix, Hk+1Representing a state observation matrix, Kk+1Is a system noise coefficient matrix, Wk+1To observe the noise coefficient matrix, deltak+1Noise, ε, representing the angular increment within an IMU sampling intervalk+1Representing the noise measured by the three-axis accelerometer, which is specifically defined as follows:
Figure BDA0002123934930000045
in the formula (6), Δ θk+1Representing the time t fromkTo tk+1Angular increment vector, delta theta, measured by a three-axis gyroscopek+1Is a vector Δ θk+1Die of (I)mAn identity matrix representing m × m; gnIs the projection of the local gravity under the navigation coordinate system;
Figure BDA0002123934930000046
is a time tk+1The output vector of the triaxial accelerometer under the moment carrier coordinate system; sk+1And vk+1Are respectively four elements q of attitudek+1A scalar part and a vector part.
Step S4.2.2: obtaining the attitude estimation corrected at the current moment according to a discrete quaternion Kalman filtering model, wherein a state vector X in the modelk+1＝qk+1In the filtering process, noise deltak+1Covariance matrix ofQk+1And noise epsilonk+1Of the covariance matrix Rk+1Calculated as follows:
Figure BDA0002123934930000051
in formula (7), σ1Standard deviation, σ, representing the angular incremental error measured by a three-axis gyroscope within an IMU sampling interval2Standard deviation, M, representing the measured noise of a triaxial accelerometerk|kAnd Mk+1|kFor intermediate variables, it is calculated as follows:
Figure BDA0002123934930000052
in the formula (8), qk|kFor the last attitude estimation of Kalman filtering, qk+1|kFor attitude prediction of the current model, Pk|kAnd Pk+1/kAre each qk|kAnd q isk+1|kThe covariance matrix of (2).
In one embodiment, the method further comprises:
and carrying out error compensation on the triaxial accelerometer.
One or more technical solutions in the embodiments of the present application have at least one or more of the following technical effects:
the method comprises the steps of firstly, installing an inertia measurement unit on a foot of a pedestrian, initializing an initial posture of the inertia measurement unit under a navigation coordinate system, then judging whether the pedestrian is in a static state or not according to the output of an inertia sensor, when the pedestrian is in a non-static state, updating the posture by adopting a strapdown inertial navigation algorithm according to the initial posture and the measurement of a three-axis gyroscope, recursion to obtain the current posture, when the pedestrian is in the static state, constructing a dynamic model according to a posture update equation of the strapdown inertial navigation algorithm, constructing a measurement model according to the output of an accelerometer and the gravity relation, and updating the current posture through a Kalman filter under a quaternion frame.
The invention constructs the attitude estimation observed quantity by using the measurement of the accelerometer at the static moment, and executes the Kalman filter to perform data fusion under the four-element framework to obtain accurate attitude information, thereby providing a simple and feasible pedestrian navigation attitude estimation method independent of an external attitude sensor. The method solves the technical problem that the method in the prior art is difficult to provide accurate attitude information.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and those skilled in the art can also obtain other drawings according to the drawings without creative efforts.
Fig. 1 is a flowchart of an indoor pedestrian navigation attitude estimation method based on a foot-worn inertial measurement unit according to the present invention;
FIG. 2 is a flow chart of an indoor pedestrian navigation attitude estimation method based on a foot-worn inertial measurement unit according to an embodiment of the present invention;
FIG. 3 is a flow chart of a four-element Kalman filtering in an embodiment of the present invention.
Detailed Description
The embodiment of the invention provides an indoor pedestrian navigation attitude estimation method based on a foot-worn inertial measurement unit, which is used for solving the technical problem that the method in the prior art is difficult to provide accurate attitude information.
The technical scheme in the embodiment of the application has the following general idea:
according to the method, the attitude observed quantity is constructed by utilizing the output of an accelerometer in a static state in the walking process of the pedestrian, and the attitude estimation of the strapdown inertial navigation algorithm is corrected through a Kalman filter under a four-element frame, so that the attitude of the pedestrian in any navigation coordinate system can be effectively estimated.
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The embodiment provides an indoor pedestrian navigation attitude estimation method based on a foot-worn inertial measurement unit, please refer to fig. 1, and the method includes:
step S1: the method comprises the steps of installing an inertia measurement unit on the foot of a pedestrian, determining a navigation coordinate system, carrying out online calibration on zero offset error of the inertia measurement unit, and initializing the initial posture of the inertia measurement unit under the navigation coordinate system, wherein the Z axis of the navigation coordinate system is vertical to the ground.
In particular, the navigation coordinate system is a framework of navigation parameter solution. In the outdoor environment, a northeast geographic coordinate system is usually used as a navigation coordinate system, and in the invention, a self-defined three-dimensional rectangular coordinate system is used as the navigation coordinate system. The selection of the self-defined coordinate system avoids the complex initialization work related by using the terrestrial coordinate system as the navigation coordinate system on one hand, and simplifies the work required by converting the navigation parameters into the indoor map coordinate system on the other hand. The invention has the following requirements on the self-defined coordinate system: the Z axis of the self-defined coordinate system is vertical to the ground. In order to save the conversion work between the navigation parameters and the map coordinates, the 0XY plane of the user-defined three-dimensional rectangular coordinate system can be coincided with the map coordinate system.
In one embodiment, step S1 specifically includes:
step S1.1: the method comprises the following steps of (1) installing an inertia measurement unit on a foot of a pedestrian, and defining a three-dimensional rectangular coordinate system as a navigation coordinate system to ensure that a Z axis of the navigation coordinate system is vertical to the ground;
step S1.2: for the three-axis gyroscope, the average value of all axial outputs in a preset time period in a static state is used as the zero offset error of the axis. For a triaxial accelerometer, the calibration is carried out by the acceleration output in a preset time at four different positions in a static state, the calibration formula is shown as formula (1),
Figure BDA0002123934930000071
wherein, bx,by,bzRespectively, the on-axis zero offset error, x, of the three-axis accelerometer X, Y, Zi、yi、ziI is 1,2,3,4, and represents the average value of X, Y, Z outputs at the i-th position, respectively, and m isiI is 1,2,3,4, which represents the sum of the squares of the average output values of the axes at the i-th position;
step S1.3: at the initial navigation time, adjusting the position of the inertial measurement unit to align the sensor coordinate system of the inertial measurement unit with the navigation coordinate system, thereby obtaining the initial attitude of the inertial measurement unit under the navigation coordinate system, wherein the initial attitude is in the form of q0＝[1,0,0,0]T。
Specifically, in the pedestrian navigation, the inertial measurement unit used is usually a low-cost inertial device, and even if calibration is performed before shipment, due to poor zero-offset error stability and poor repeatability of the three-axis accelerometer and the three-axis gyroscope, the error needs to be calibrated online again before each use, so as to improve the measurement accuracy of the sensor. The online calibration method of the triaxial gyroscope comprises the following steps: and taking the average value of each axial output in preset time in a static state as the zero offset error of the shaft. For the accelerometer, due to the influence of gravity, calibration cannot be carried out according to the method, and calibration can be carried out through the output of the accelerometer in preset time at four different positions in a static state. The preset time can be selected according to actual conditions and experience.
After the navigation coordinate system is determined, the initial attitude of the inertial measurement unit in the navigation coordinate system can be simply determined as follows: at an initial navigation time, the position of the inertial measurement unit is adjusted. The invention aligns the inertial measurement unit sensor coordinate system with the navigation coordinate system so that the initial attitude is expressed as q in the form of four elements0＝[1,0,0,0]T。
Step S2: and judging whether the pedestrian is in a static state or not according to the output of the three-axis gyroscope, if so, executing step S3, and otherwise, executing step S4.
In one embodiment, step S2 specifically includes:
step S2.1: judging whether the total length of the sequence output by the current inertia measurement unit is greater than or equal to a preset length N, if so, turning to the step S2.2, and judging whether the pedestrian is in a static state;
step S2.2: calculating the modulo y of the current triaxial gyroscope measurement vectork-b | |, wherein ykB is the zero-offset vector of the three-axis gyroscope obtained in step S1, and determines yk-whether b | | is greater than a set first threshold value thmIf the pedestrian is in the non-stationary state, judging that the pedestrian is in the non-stationary state at the current moment, otherwise, executing the step S2.3;
step S2.3: further calculating the variance var of the gyroscope measurement vector mode in the fixed-length window, wherein the calculation formula is shown as formula (2):
Figure BDA0002123934930000081
wherein c represents the mean value of the N gyroscope measurement vector moduli in the window, and if var is greater than a set second threshold thvAnd if not, determining that the pedestrian is in the static state at the current moment.
Specifically, please refer to fig. 2, which is a flowchart illustrating a method for estimating a navigation posture of an indoor pedestrian according to a specific example. The pedestrian static state detection method detects the static state of the pedestrian according to the output of the inertial sensor, the static state of the pedestrian is detected by adopting the current three-axis gyroscope output value amplitude and the gyroscope output value amplitude variance in the fixed-length window, and the length of the window is assumed to be N. If the total length of the current inertial sensor output sequence is less than N and the stationary detection cannot be performed, go to step S3 to execute. Otherwise, a determination is made by step S2.2 and step S2.3.
In step S2, the first threshold thmAnd a second threshold thvThe following is selected as follows: according to the static gyroscope data acquired for calibrating the zero-offset error of the three-axis gyroscope in the step S1, calculating the modulus of all gyroscope measurement vectors in the group of data, and finding the maximum modulus value with 1.5 times as thm(ii) a Starting from the Nth data, calculating the variance of a data set consisting of the modulus of each gyroscope measuring vector and the modulus of the first N-1 gyroscope measuring vectors, finding the maximum value of the variance, and taking 1.5 times of the maximum value as thv。
In step S2, the window length N may be set according to the resting time period during a full gait of the pedestrian during normal walking. A large number of experimental test results of different scholars show that the standing time in a complete gait is about 0.3-0.5 second during the normal walking period of the pedestrian. Thus, N may be set to 0.3F, where F represents the output frequency of the inertial sensor. The inventor of the application finds out through a large amount of practice and research that: n cannot be set too large, which can cause the detection omission of the real static state; n is also not easy to set too small, which would detect multiple static states in one gait, resulting in increased computational overhead.
Of course, in the specific implementation, the detection of the static state may be implemented in other manners, and the present invention is not limited to this.
Step S3: and updating the attitude by adopting a strapdown inertial navigation algorithm according to the initial attitude and the measurement of the triaxial gyroscope, and recurrently obtaining the current attitude, wherein the measurement of the triaxial gyroscope is taken as the rotation angular velocity of the inertial measurement unit relative to the navigation coordinate system under the sensor coordinate system without considering the influence of the autorotation of the earth.
Specifically, according to a traditional strapdown inertial navigation algorithm, the current posture is obtained through recursion. In the recursive algorithm, the rotation angular velocity of the inertial measurement unit relative to the navigation coordinate system needs to be measured in the sensor coordinate system, and the output of the three-axis gyroscope in the inertial measurement unit is measured as the rotation angular velocity of the inertial measurement unit relative to the navigation coordinate system in the sensor coordinate system, so the influence of the earth rotation needs to be deducted from the rotation angular velocity. Under a self-defined navigation coordinate system, earth rotation angular velocity compensation of the output of the gyroscope measured by the three-axis gyroscope is very difficult. But the rotational angular velocity of the earth is very small, about 0.004 degree per second, so that the rotational angular velocity can be ignored.
In one embodiment, step S3 specifically includes:
and (3) recursion is carried out according to a traditional strapdown inertial navigation algorithm to obtain the current posture, and the strapdown posture updating equation is as follows:
Figure BDA0002123934930000091
wherein w (t) is the rotation angular velocity of the inertial measurement unit relative to the navigation coordinate system under the sensor coordinate system, qk+1Represents tk+1Attitude four elements of time, qkRepresents tkAnd four elements of the posture of the moment.
Specifically, after the recursive gesture update is completed, it is determined whether the navigation is completed, and if so, the gesture update is completed, otherwise, the process returns to step S2.
Step S4: and constructing a dynamic model according to a strapdown inertial navigation algorithm attitude updating equation, constructing a measurement model according to the relation between accelerometer output and gravity, and updating the current attitude through a Kalman filter under a quaternion frame.
Specifically, the attitude update is performed by a kalman filter under a quaternion framework. The current attitude can be obtained according to the strapdown inertial navigation algorithm, but the attitude has larger error due to sensor error. The invention utilizes the three-axis accelerometer in the static state of the pedestrian to construct the attitude observed quantity to correct the current attitude.
Wherein, step S4 specifically includes:
step S4.1: establishing the relation between the real measuring vector (not considering zero offset and other errors) of the current triaxial accelerometer and the attitude estimation under the sensor coordinate system,
Figure BDA0002123934930000101
wherein the content of the first and second substances,
Figure BDA0002123934930000102
quaternion form, q, representing the projection of local gravity under the navigation coordinate systemk+1Is a time tk+1The posture of the time is four elements,
Figure BDA0002123934930000103
is a time tk+1A quaternion form of a real measurement vector of the triaxial accelerometer under a time sensor coordinate system; an as quaternion multiplication operator;
step S4.2: and (3) constructing a dynamic model by using a strapdown attitude updating equation, constructing a measurement model by using an equation (4), and estimating the current attitude information by using a Kalman filter.
Specifically, please refer to fig. 3, which is a four-element kalman filtering flowchart. The current posture can be recurred according to the formula in step S3. However, the attitude estimation error thus obtained is large due to the sensor error, and it is necessary to correct the error using a reliable external observation amount. In a pedestrian stationary state, the measurement of the tri-axial accelerometer reflects only the influence of the earth gravity (i.e., the measurement is equal to the projection of gravity in the current coordinate system), while the projection of gravity in the navigation coordinate system is known, so that the relationship between the current tri-axial accelerometer measurement and the attitude estimation can be established.
Figure BDA0002123934930000104
In the form of a quaternion of the projection of the local gravity on the navigation coordinate system, the projection of the gravity on the navigation coordinate system is known according to the definition of the navigation coordinate system. The three-dimensional vector can be converted into a quaternion form by supplementing a 0 element, for example, if the navigation coordinate system is vertically upwards, the projection of gravity under the navigation coordinate system is gn＝[0 0 -g]TRewritten into a quaternion form
Figure BDA0002123934930000105
In the indoor navigation application, the gravity g is a constant without considering the gravity change in the navigation area.
In one embodiment, step S4.2 specifically includes:
step S4.2.1: with the attitude as the state variable, the following discrete four-element Kalman filtering model is established according to the equation (4) and the strapdown attitude update equation in the step S3,
Figure BDA0002123934930000106
in the formula (5), Fk+1Represents the state transition matrix, Hk+1Representing a state observation matrix, Kk+1Is a system noise coefficient matrix, Wk+1To observe the noise coefficient matrix, deltak+1Noise, ε, representing the angular increment within an IMU sampling intervalk+1Representing the noise measured by the three-axis accelerometer, which is specifically defined as follows:
Figure BDA0002123934930000111
in the formula (6), Δ θk+1Representing the time t fromkTo tk+1Angular increment vector, delta theta, measured by a three-axis gyroscopek+1Is a vector Δ θk+1Die of (I)mAn identity matrix representing m × m; gnIs the projection of the local gravity under the navigation coordinate system;
Figure BDA0002123934930000112
is a time tk+1The output vector of the triaxial accelerometer under the moment carrier coordinate system; sk+1And vk+1Are respectively four elements q of attitudek+1A scalar part and a vector part.
Step S4.2.2: obtaining the attitude estimation corrected at the current moment according to a discrete quaternion Kalman filtering model, wherein a state vector X in the modelk+1＝qk+1In the filtering process, noise deltak+1Of the covariance matrix Qk+1And noise epsilonk+1Of the covariance matrix Rk+1Calculated as follows:
Figure BDA0002123934930000113
in formula (7), σ1Standard deviation, σ, representing the angular incremental error measured by a three-axis gyroscope within an IMU sampling interval2Standard deviation, M, representing the measured noise of a triaxial accelerometerk|kAnd Mk+1|kFor intermediate variables, it is calculated as follows:
Figure BDA0002123934930000114
in the formula (8), qk|kFor the last attitude estimation of Kalman filtering, qk+1|kFor attitude prediction of the current model, Pk|kAnd Pk+1/kAre each qk|kAnd q isk+1|kThe covariance matrix of (2).
In particular, the system model in the filter is a nonlinear equation, and the data fusion by using the traditional Kalman filter is very complicated, so the Kalman filter is implemented under a four-element framework. And establishing a discrete four-element Kalman filtering model by taking the attitude four elements as state quantities. After the filtering is completed, the process returns to step S2 again.
Compared with the common Kalman filter, the Kalman filter implemented under the four-element framework of the invention has the following differences: the four-element Kalman filtering method and the four-element Kalman filtering method are implemented under different frames, one is a common three-dimensional vector frame, the other is a four-element frame, and the algorithms under different frames are different, so that the problem of complex linearization of the traditional Kalman filtering method can be effectively solved.
To obtain accurate and reliable three-axis acceleration measurements, in one embodiment, the method further comprises: and carrying out error compensation on the triaxial accelerometer.
In particular, the present invention takes into account the variation of the error of the tri-axial accelerometer over time. In the specific implementation process, any feasible method may be adopted to perform the error compensation of the triaxial accelerometer, which is not particularly limited in this discovery. In the present embodiment, the following compensation method is provided. Unifying various error influences of the triaxial accelerometer into a zero-offset error, establishing a zero-offset error random walk model,
bk+1＝bk+ηk+1 (9)
in the formula (9), bk+1And bkRepresenting the zero offset error, η, of the triaxial accelerometer at the current time and at the previous timek+1Is the current noise. By adopting a zero-speed updating method, the speed of the pedestrian in a static state is used as an observed quantity, the speed of the pedestrian and the deviation of the three-axis accelerometer are used as state quantities, the following discrete Kalman filtering model can be established for estimating the zero offset of the three-axis accelerometer,
Figure BDA0002123934930000121
in the formula (10), xkIs a state vector consisting of the current speed of the pedestrian and the deviation of the triaxial accelerometer, Ak+1Is a state transition matrix, Bk+1Is a state observation matrix, ωk+1And
Figure BDA0002123934930000122
respectively, system noise and observation noise. A. thek+1And Bk+1The definition is as follows,
Figure BDA0002123934930000123
where T is the inertial sensor unit output time interval,
Figure BDA0002123934930000124
direction cosine matrix representing the sensor coordinate system to the navigation coordinate system, I3×1And 03×1Respectively representing three-dimensional column vectors with all elements of 1 and three-dimensional column vectors with all elements of 0. Four elementsAnd after the Kalman filtering is finished, judging whether the navigation is finished or not, if so, finishing the attitude updating, and if not, returning to the step S2.
One or more technical solutions in the embodiments of the present application have at least one or more of the following technical effects:
the invention constructs the attitude estimation observed quantity by using the measurement of the accelerometer at the static moment, and executes the Kalman filter to perform data fusion under the four-element framework to obtain accurate attitude information, thereby providing a simple and feasible pedestrian navigation attitude estimation method independent of an external attitude sensor.
While preferred embodiments of the present invention have been described, additional variations and modifications in those embodiments may occur to those skilled in the art once they learn of the basic inventive concepts. Therefore, it is intended that the appended claims be interpreted as including preferred embodiments and all such alterations and modifications as fall within the scope of the invention.
It will be apparent to those skilled in the art that various modifications and variations can be made in the embodiments of the present invention without departing from the spirit or scope of the embodiments of the invention. Thus, if such modifications and variations of the embodiments of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to encompass such modifications and variations.

Claims (5)

1. Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit is characterized by comprising the following steps:
step S1: the method comprises the steps of installing an inertia measurement unit on the foot of a pedestrian, determining a navigation coordinate system, carrying out online calibration on zero offset error of the inertia measurement unit, and initializing the initial posture of the inertia measurement unit under the navigation coordinate system, wherein the Z axis of the navigation coordinate system is vertical to the ground;
step S2: judging whether the pedestrian is in a static state according to the output of the inertial sensor, if so, executing step S3, otherwise, executing step S4;
step S3: according to the initial attitude and the output of the triaxial gyroscope, adopting a strapdown inertial navigation algorithm to update the attitude, and recursion to obtain the current attitude, wherein the measurement of the triaxial gyroscope is used as the rotation angular velocity of an inertial measurement unit relative to a navigation coordinate system under a sensor coordinate system;
step S4: constructing a dynamic model according to a strapdown inertial navigation algorithm attitude updating equation, constructing a measurement model according to the relation between accelerometer output and gravity, and updating the current attitude through a Kalman filter under a quaternion frame;
wherein, step S4 specifically includes:
step S4.1: establishing the relation between the real measurement vector of the acceleration and the attitude estimation under the current sensor coordinate system,
Figure FDA0002847323790000011
wherein the content of the first and second substances,
Figure FDA0002847323790000012
quaternion form, q, representing the projection of local gravity under the navigation coordinate systemk+1Is a time tk+1The quaternion of the attitude at the moment,
Figure FDA0002847323790000013
is a time tk+1A quaternion form of a real measurement vector of the triaxial accelerometer under a time sensor coordinate system; an as quaternion multiplication operator;
step S4.2: constructing a dynamic model by using a strapdown attitude updating equation, constructing a measurement model by using a formula (4), and estimating current attitude information by using a Kalman filter;
step S4.2 specifically includes:
step S4.2.1: with the attitude as the state variable, the following discrete quaternion Kalman filtering model is established according to the equation (4) and the strapdown attitude update equation in the step S3,
Figure FDA0002847323790000014
in the formula (5), Fk+1Represents the state transition matrix, Hk+1Representing a state observation matrix, Kk+1Is a system noise coefficient matrix, Wk+1To observe the noise coefficient matrix, deltak+1Noise, ε, representing the angular increment within an IMU sampling intervalk+1Representing the noise measured by the three-axis accelerometer, which is specifically defined as follows:
Figure FDA0002847323790000021
in the formula (6), Δ θk+1Representing the time t fromkTo tk+1Angular increment vector, delta theta, measured by a three-axis gyroscopek+1Is a vector Δ θk+1Die of (I)mAn identity matrix representing m × m; gnIs the projection of the local gravity under the navigation coordinate system;
Figure FDA0002847323790000022
is a time tk+1The output vector of the triaxial accelerometer under the moment carrier coordinate system; sk+1And vk+1Are respectively attitude quaternions qk+1A scalar part and a vector part;
step S4.2.2: obtaining the attitude estimation corrected at the current moment according to a discrete quaternion Kalman filtering model, wherein a state vector X in the modelk+1＝qk+1In the filtering process, noise deltak+1Of the covariance matrix Qk+1And noise epsilonk+1Of the covariance matrix Rk+1Calculated as follows:
Figure FDA0002847323790000023
in formula (7), σ1Standard deviation, sigma, representing the error of angular increment measured by a triaxial gyroscope within a sampling interval of an inertial measurement unit2Standard deviation, M, representing the measured noise of a triaxial accelerometerk|kAnd Mk+1|kAs an intermediate variableCalculated as follows:
Figure FDA0002847323790000024
in the formula (8), qk|kFor the last attitude estimation of Kalman filtering, qk+1|kFor attitude prediction of the current model, Pk|kAnd Pk+1/kAre each qk|kAnd q isk+1|kThe covariance matrix of (2).
2. The method according to claim 1, wherein step S1 specifically comprises:
step S1.1: the method comprises the following steps of (1) installing an inertia measurement unit on a foot of a pedestrian, and defining a three-dimensional rectangular coordinate system as a navigation coordinate system to ensure that a Z axis of the navigation coordinate system is vertical to the ground;
step S1.2: for a three-axis gyroscope, taking the average value of each axial output in a preset time period in a static state as the zero offset error of the axis; for a triaxial accelerometer, the calibration is carried out by the acceleration output in a preset time at four different positions in a static state, the calibration formula is shown as formula (1),
Figure FDA0002847323790000031
wherein, bx,by,bzRespectively, the on-axis zero offset error, x, of the three-axis accelerometer X, Y, Zi、yi、ziI is 1,2,3,4, and represents the average value of X, Y, Z outputs at the i-th position, respectively, and m isiI is 1,2,3,4, which represents the sum of the squares of the average output values of the axes at the i-th position;
step S1.3: at the initial navigation time, adjusting the position of the inertial measurement unit to align the sensor coordinate system of the inertial measurement unit with the navigation coordinate system, thereby obtaining the initial attitude of the inertial measurement unit under the navigation coordinate system, wherein the initial attitude is in the form of q0＝[1,0,0,0]T。
3. The method according to claim 1, wherein step S2 specifically comprises:
step S2.1: judging whether the total length of the sequence output by the current inertia measurement unit is greater than or equal to a preset length N, if so, turning to the step S2.2, and judging whether the pedestrian is in a static state;
step S2.2: calculating the modulo y of the current triaxial gyroscope measurement vectork-b | |, wherein ykB is the zero-offset vector of the three-axis gyroscope obtained in step S1, and determines yk-whether b | | is greater than a set first threshold value thmIf the pedestrian is in the non-stationary state, judging that the pedestrian is in the non-stationary state at the current moment, otherwise, executing the step S2.3;
step S2.3: further calculating the variance var of the gyroscope measurement vector mode in the fixed-length window, wherein the calculation formula is shown as formula (2):
Figure FDA0002847323790000032
wherein c represents the mean value of the N gyroscope measurement vector moduli in the window, and if var is greater than a set second threshold thvAnd if not, determining that the pedestrian is in the static state at the current moment.
4. The method according to claim 1, wherein step S3 specifically comprises:
and (3) recursion is carried out according to a traditional strapdown inertial navigation algorithm to obtain the current posture, and the strapdown posture updating equation is as follows:
Figure FDA0002847323790000041
wherein w (t) is the rotation angular velocity of the inertial measurement unit relative to the navigation coordinate system under the sensor coordinate system, qk+1Represents tk+1Attitude quaternion of time, qkRepresents tkAttitude quaternion of the time of day.
5. The method of any one of claims 1-4, further comprising:
and carrying out error compensation on the triaxial accelerometer.
CN201910615908.6A 2019-07-09 2019-07-09 Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit Active CN110398245B (en)

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 CN110398245A (en) 2019-11-01
CN110398245B true 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)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110887481B (en) * 2019-12-11 2020-07-24 中国空气动力研究与发展中心低速空气动力研究所 Carrier dynamic attitude estimation method based on MEMS inertial sensor
CN110916577B (en) * 2019-12-17 2021-11-09 小狗电器互联网科技（北京）股份有限公司 Robot static state judgment method and device and robot
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
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
CN112781622B (en) * 2020-12-31 2022-07-05 厦门华源嘉航科技有限公司 Pedestrian navigation MIMU installation error online calibration method
CN113008230B (en) * 2021-02-26 2024-04-02 广州市偶家科技有限公司 Intelligent wearable device and gesture direction recognition method and device thereof
CN112945240B (en) * 2021-03-16 2022-06-07 北京三快在线科技有限公司 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
CN114563018A (en) * 2022-03-04 2022-05-31 闪耀现实(无锡)科技有限公司 Method and apparatus for calibrating head mounted display device

Citations (4)

* Cited by examiner, † Cited by third party
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
CN106705968A (en) * 2016-12-09 2017-05-24 北京工业大学 Indoor inertial navigation algorithm based on posture recognition and step length model
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9002565B2 (en) * 2003-03-20 2015-04-07 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
CN106949889A (en) * 2017-03-17 2017-07-14 南京航空航天大学 For the inexpensive MEMS/GPS integrated navigation systems and method of pedestrian navigation

Patent Citations (4)

* Cited by examiner, † Cited by third party
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
CN106705968A (en) * 2016-12-09 2017-05-24 北京工业大学 Indoor inertial navigation algorithm based on posture recognition and step length model
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device

Also Published As

Publication number Publication date
CN110398245A (en) 2019-11-01

Similar Documents

Publication Publication Date Title
CN110398245B (en) Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit
CN101726295B (en) Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation
CN112013836B (en) Attitude heading reference system algorithm based on improved adaptive Kalman filtering
CN110398257B (en) GPS-assisted SINS system quick-acting base initial alignment method
Rohac et al. Calibration of low-cost triaxial inertial sensors
CN106979780B (en) A kind of unmanned vehicle real-time attitude measurement method
CN107270893B (en) Lever arm and time asynchronous error estimation and compensation method for real estate measurement
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
Phuong et al. A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass
CN106500693B (en) A kind of AHRS algorithm based on adaptive extended kalman filtering
US9417091B2 (en) System and method for determining and correcting field sensors errors
CN107490378B (en) Indoor positioning and navigation method based on MPU6050 and smart phone
CN108036785A (en) A kind of aircraft position and orientation estimation method based on direct method and inertial navigation fusion
CN109099913B (en) Wearable navigation device and method based on MEMS inertial device
CN104713554A (en) Indoor positioning method based on MEMS insert device and android smart mobile phone fusion
CN103822633A (en) Low-cost attitude estimation method based on second-order measurement update
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
CN112798021B (en) Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter
CN110174123B (en) Real-time calibration method for magnetic sensor
CN109764870B (en) Carrier initial course estimation method based on transformation estimation modeling scheme
WO2022160391A1 (en) Magnetometer information assisted mems gyroscope calibration method and calibration system
CN111189474A (en) Autonomous calibration method of MARG sensor based on MEMS
CN106403952A (en) Method for measuring combined attitudes of Satcom on the move with low cost
CN110672095A (en) Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation
CN112362057A (en) Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation

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