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

Abstract

The invention provides an indoor pedestrian navigation attitude estimation method based on a foot-mounted inertial measurement unit. First, the inertial measurement unit is installed on the foot of the pedestrian, the initial attitude of the inertial measurement unit in the navigation coordinate system is initialized, and then according to the inertial sensor The output judges whether the pedestrian is in a stationary state. When the pedestrian is in a non-stationary state, the SINS algorithm is used to update the attitude, and the current attitude is obtained recursively. When the pedestrian is in a stationary state, the attitude update equation is based on the SINS algorithm. The dynamic model, the relationship between the accelerometer output and the gravity are constructed to construct the measurement model, and the current attitude is updated through the Kalman filter under the quaternion frame. The invention utilizes the measurement of the accelerometer at the static moment to construct the attitude estimation observation quantity, executes the Kalman filter under the four-element framework to perform data fusion to obtain precise attitude information, and realizes the technical effect of providing precise attitude information.

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-mounted inertial measurement unit.

Background technique

"Pedestrian navigation" refers to providing the necessary location and direction information on the basis of the map to guide pedestrians to reach the target. In the constantly enriched smart electronic devices, pedestrian navigation has become an important function (such as pedestrian navigation services provided by AutoNavi Maps and Baidu Maps in smartphones), which has greatly improved people's travel efficiency.

Outdoors, accurate location information can be obtained by relying on the Global Navigation Satellite System (GNSS), and combined with other orientation sensors (such as gyroscopes), accurate pedestrian navigation can be achieved. However, indoors, due to the occlusion of building structures and walls, the GNSS signal is severely attenuated, and effective location information cannot be estimated. Therefore, indoor pedestrian navigation faces more difficulties and needs to develop special technologies, which is one of the hotspots in the field of positioning and navigation at home and abroad.

In the prior art, available indoor position estimation techniques can be roughly classified into vision-based methods, wireless signal-based methods, and inertial sensor-based methods. Considering the problems of cost, power consumption, and computational complexity, the method based on the foot-mounted Inertial Measurement Unit (IMU) is widely used in indoor pedestrian navigation position estimation. How to obtain accurate direction information is the main difficulty of indoor pedestrian navigation. Relying on the measurement of the built-in angular velocity sensor of the IMU for direction estimation is an economical and simple method, but there is a problem of direction drift, which needs to be continuously corrected to limit the direction drift. Current solutions include zero-acceleration update algorithms, magnetic heading assist algorithms, and more.

In the process of implementing the present invention, the inventor of the present application found that the method of the prior art has at least the following technical problems:

The zero-acceleration update algorithm depends on the attitude error model used, and is only valid in the earth coordinate system; the magnetic heading assistance algorithm requires accurate heading observations, but due to the severe indoor geomagnetic interference, the heading estimation obtained by the geomagnetic measurement will have a large amount of space. error. In addition, these solutions need to be carried out in the earth coordinate system, which greatly increases the initialization work of the navigation system; moreover, these solutions generally only provide heading information, but cannot provide accurate three-dimensional direction information (ie attitude). For some special indoor pedestrian navigation applications, such as firefighter navigation, posture information is helpful to monitor the walking state of the navigated person (standing, lying down, squatting, etc.), so as to infer whether it is safe.

It can be seen from this that the methods in the prior art have the technical problem that it is difficult to provide accurate attitude information.

SUMMARY OF THE INVENTION

In order to solve the problem that the current indoor pedestrian navigation system is difficult to provide accurate attitude information, the present invention provides an indoor pedestrian navigation attitude estimation method based on a foot-mounted IMU. The Kalman filter is executed under the quaternion frame to correct the attitude estimation of the IMU strapdown algorithm.

The present invention provides an indoor pedestrian navigation attitude estimation method based on a foot-mounted IMU, including:

Step S1: Install the inertial measurement unit on the foot of the pedestrian, determine the navigation coordinate system, perform online calibration on the zero offset error of the inertial measurement unit, and initialize the initial attitude of the inertial measurement unit in the navigation coordinate system, wherein the Z of the navigation coordinate system is The axis is perpendicular to the ground;

Step S2: determine whether the pedestrian is in a stationary state according to the output of the inertial sensor, if the pedestrian is in a non-stationary state, perform step S3, otherwise, perform step S4;

Step S3: According to the initial attitude and the output of the three-axis gyroscope, the strapdown inertial navigation algorithm is used to update the attitude, and the current attitude is obtained by recursion, wherein the measurement of the three-axis gyroscope is used as the relative position of the inertial measurement unit in the sensor coordinate system. The rotational angular velocity of the navigation coordinate system;

Step S4: constructing a dynamic model according to the attitude update equation of the strapdown inertial navigation algorithm, constructing a measurement model based on the relationship between the output of the accelerometer and the gravity, and updating the current attitude through the Kalman filter under the quaternion frame.

In one embodiment, step S1 specifically includes:

Step S1.1: Install the inertial measurement unit on the foot of the pedestrian, customize the three-dimensional rectangular coordinate system as the navigation coordinate system, and ensure that the Z axis of the navigation coordinate system is perpendicular to the ground;

Step S1.2: For a three-axis gyroscope, the average value of the output of each axis in a predetermined period of time in a static state is used as the zero offset error of the axis. For a three-axis accelerometer, the calibration is performed by the output of the accelerometer at four different positions in a stationary state for a preset period of time. The calibration formula is shown in formula (1),

Among them, b _x , b _y , b _z are the zero bias errors on the X, Y, and Z axes of the three-axis accelerometer, respectively, x _i , y _i , z _i , i=1, 2, 3, 4, representing the i-th accelerometer, respectively The average value of X, Y, Z outputs at each position, m _i , i=1, 2, 3, 4 represents the sum of the squares of the average output values of each axis at the ith position;

Step S1.3: At the initial navigation moment, adjust the position of the inertial measurement unit to align the sensor coordinate system of the inertial measurement unit with the navigation coordinate system, so as to obtain the initial attitude of the inertial measurement unit in the navigation coordinate system, wherein the initial attitude is in the form of is q ₀ =[1,0,0,0] ^T .

In one embodiment, step S2 specifically includes:

Step S2.1: determine whether the total length of the sequence output by the current inertial measurement unit is greater than or equal to the preset length N, and if so, go to step S2.2 to determine whether the pedestrian is in a stationary state;

Step S2.2: Calculate the modulus of the current three-axis gyroscope measurement vector ||y _k -b||, where y _k is the current three-axis gyroscope measurement vector, and b is the zero value of the three-axis gyroscope obtained in step S1 bias vector, and determine whether ||y _k -b|| is greater than the set first threshold th _m , if it is greater, then determine that the pedestrian is in a non-stationary state at the current moment, otherwise, go to step S2.3;

Step S2.3: further calculate the variance var of the vector mode measured by the gyroscope in the fixed-length window, and the calculation formula is shown in formula (2):

Among them, c represents the mean value of the vector moduli measured by N gyroscopes in the window. If var is greater than the set second threshold th _v , it is determined that the pedestrian is in a non-stationary state at the current moment, otherwise, it is determined that the pedestrian is in a stationary state at the current moment.

In one embodiment, step S3 specifically includes:

According to the traditional strapdown inertial navigation algorithm, the current attitude is obtained recursively. The strapdown attitude update equation is as follows:

Among them, w(t) is the rotational angular velocity of the inertial measurement unit in the sensor coordinate system relative to the navigation coordinate system, q _k+1 represents the four elements of attitude at time t _k+1 , and q _k+1 represents the four elements of attitude at time t _k .

In one embodiment, step S4 specifically includes:

Step S4.1: Establish the relationship between the real measurement vector of the three-axis accelerometer and the attitude estimation in the current sensor coordinate system,

表示当地重力在导航坐标系下投影的四元数形式，q_k+1为时间t_k+1时刻姿态四元素，

为时间t_k+1时刻传感器坐标系下三轴加速度计的真实测量向量的四元数形式；⊙为四元数乘法运算符；in,

represents the quaternion form of the projection of local gravity in the navigation coordinate system, q _k+1 is the four elements of attitude at time t _k+1 ,

is the quaternion form of the real measurement vector of the three-axis accelerometer in the sensor coordinate system at time tk ₊₁ ; ⊙ is the quaternion multiplication operator;

Step S4.2: Construct the dynamic model by the strapdown attitude update equation, construct the measurement model by the formula (4), and use the Kalman filter to estimate the current attitude information.

In one embodiment, step S4.2 specifically includes:

Step S4.2.1: Taking the attitude as the state variable, the following discrete four-element Kalman filter model is established according to equation (4) and the strapdown attitude update equation in step S3,

In formula (5), F _k+1 is the state transition matrix, H _k+1 is the state observation matrix, K _k+1 is the system noise coefficient matrix, W _k+1 is the observation noise coefficient matrix, δ _k+1 is the IMU The noise of the angular increment within the sampling interval, ε _k+1 represents the noise of the triaxial accelerometer measurement, which is specifically defined as follows:

为时间t_k+1时刻载体坐标系下三轴加速度计的输出向量；s_k+1和v_k+1分别是姿态四元素q_k+1的标量部分和矢量部分。In formula (6), Δθ _k+1 represents the angular increment vector measured by the three-axis gyroscope from time t _k to t _k+1 , Δθ _k+1 is the modulus of the vector Δθ _k+1 , and I _m represents m× The identity matrix of m; g ⁿ is the projection of the local gravity in the navigation coordinate system;

is the output vector of the three-axis accelerometer in the carrier coordinate system at time t _k+1 ; s _k+1 and v _k+1 are the scalar part and the vector part of the four-element attitude q _k+ 1 respectively.

Step S4.2.2: According to the discrete four-element Kalman filter model, the corrected attitude estimation at the current moment is obtained, in which, the state vector X _k+1 =q _k+1 in the model, in the filtering process, the co-ordination of noise δ _k+1 is The covariance matrix R _k+1 of the square matrix Q _k+1 and the noise ε _k+1 is calculated as follows:

In formula (7), σ ₁ represents the standard deviation of the angular increment error measured by the three-axis gyroscope within the sampling interval of the IMU, σ ₂ represents the standard deviation of the measurement noise of the three-axis accelerometer, M _k|k and M _{k+1 |k} is an intermediate variable, calculated as follows:

In formula (8), q _k|k is the last attitude estimation of the Kalman filter, q _k+1|k is the attitude prediction of the current model, P _k|k and P _k+1/k are q _k|k and q _k+1|k covariance matrix.

In one embodiment, the method further includes:

Error compensation for three-axis accelerometers.

The above-mentioned one or more technical solutions in the embodiments of the present application have at least one or more of the following technical effects:

In the method provided by the present invention, firstly, the inertial measurement unit is installed on the foot of the pedestrian, the initial attitude of the inertial measurement unit in the navigation coordinate system is initialized, and then it is judged whether the pedestrian is in a stationary state according to the output of the inertial sensor. In the static state, according to the initial attitude and the measurement of the three-axis gyroscope, the SINS algorithm is used to update the attitude, and the current attitude is obtained recursively. The model, accelerometer output and gravity relationship construct the measurement model, and update the current attitude through Kalman filter under the quaternion frame.

The invention uses the measurement of the accelerometer at the static moment to construct the attitude estimation observation, and executes the Kalman filter under the four-element framework to perform data fusion to obtain accurate attitude information, thereby providing a simple and feasible method that does not depend on external attitude sensors. Pedestrian Navigation Pose Estimation Method. The technical problem that the methods in the prior art are difficult to provide accurate attitude information is solved.

Description of drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the accompanying drawings that need to be used in the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description These are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can also be obtained according to these drawings without creative efforts.

1 is a flowchart of an indoor pedestrian navigation attitude estimation method based on a foot-mounted inertial measurement unit provided by the present invention;

2 is a flowchart of an indoor pedestrian navigation attitude estimation method based on a foot-mounted inertial measurement unit in a specific example of the present invention;

FIG. 3 is a flowchart of four-element Kalman filtering in an embodiment of the present invention.

Detailed ways

The embodiments of the present invention provide an indoor pedestrian navigation attitude estimation method based on a foot-mounted inertial measurement unit, which is used to improve the technical problem that it is difficult to provide accurate attitude information in the method in the prior art.

The technical scheme in the embodiment of the present application, the general idea is as follows:

Provided is an indoor pedestrian navigation attitude estimation method. The method uses the accelerometer output in the stationary state during pedestrian walking to construct attitude observations, and under the four-element framework, the attitude estimation of the strapdown inertial navigation algorithm is corrected by Kalman filter. , which can realize the effective estimation of pedestrian attitude in any navigation coordinate system.

In order to make the purposes, 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 accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments These are some embodiments of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

This embodiment provides an indoor pedestrian navigation attitude estimation method based on a foot-mounted inertial measurement unit. Please refer to FIG. 1 , and the method includes:

Step S1: Install the inertial measurement unit on the foot of the pedestrian, determine the navigation coordinate system, perform online calibration on the zero offset error of the inertial measurement unit, and initialize the initial attitude of the inertial measurement unit in the navigation coordinate system, wherein the Z of the navigation coordinate system is The axis is perpendicular to the ground.

Specifically, the navigation coordinate system is the framework for the calculation of navigation parameters. Outdoors, the northeast sky geographic coordinate system is usually used as the navigation coordinate system. In the present invention, a self-defined three-dimensional rectangular coordinate system is used as the navigation coordinate system. The choice of the custom coordinate system, on the one hand, avoids the complicated initialization work involved in using the earth coordinate system as the navigation coordinate system, and on the other hand simplifies the work required to convert the navigation parameters into the indoor map coordinate system. The present invention has the following requirements for the self-defined coordinate system: the Z-axis of the self-defined coordinate system is perpendicular to the ground. In order to save the conversion work between navigation parameters and map coordinates, the 0XY plane of the custom 3D Cartesian coordinate system can be overlapped with the map coordinate system.

In one embodiment, step S1 specifically includes:

Step S1.1: Install the inertial measurement unit on the foot of the pedestrian, customize the three-dimensional rectangular coordinate system as the navigation coordinate system, and ensure that the Z axis of the navigation coordinate system is perpendicular to the ground;

Step S1.2: For a three-axis gyroscope, the average value of the output of each axis in a predetermined period of time in a static state is used as the zero offset error of the axis. For a three-axis accelerometer, it is calibrated by the acceleration output at four different positions in a stationary state for a preset period of time. The calibration formula is shown in formula (1),

Among them, b _x , b _y , b _z are the zero bias errors on the X, Y, and Z axes of the three-axis accelerometer, respectively, x _i , y _i , z _i , i=1, 2, 3, 4, representing the i-th accelerometer, respectively The average value of X, Y, Z outputs at each position, m _i , i=1, 2, 3, 4 represents the sum of the squares of the average output values of each axis at the ith position;

Step S1.3: At the initial navigation moment, adjust the position of the inertial measurement unit to align the sensor coordinate system of the inertial measurement unit with the navigation coordinate system, so as to obtain the initial attitude of the inertial measurement unit in the navigation coordinate system, wherein the initial attitude is in the form of is q ₀ =[1,0,0,0] ^T .

Specifically, in pedestrian navigation, the inertial measurement unit used is usually a low-cost inertial device. Even if it has been calibrated before leaving the factory, it has poor stability and repeatability due to the zero bias error of the three-axis accelerometer and the three-axis gyroscope. , the error needs to be re-calibrated online before each use to improve the measurement accuracy of the sensor. The online calibration method of the three-axis gyroscope is as follows: the average value of the output of each axis in a preset time in a static state is used as the zero-bias error of the axis. For the accelerometer, due to the influence of gravity, it cannot be calibrated according to the above method. It can be calibrated by the output of the accelerometer at four different positions within a preset time in a stationary state. Among them, the preset time can be selected according to the actual situation 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 the initial navigation moment, the position of the inertial measurement unit is adjusted. The present invention aligns the inertial measurement unit sensor coordinate system with the navigation coordinate system, so that the initial attitude is expressed in four-element form as q ₀ =[1,0,0,0] ^T .

Step S2: Determine whether the pedestrian is in a stationary state according to the output of the three-axis gyroscope, if the pedestrian is in a non-stationary state, perform step S3, otherwise, perform step S4.

In one embodiment, step S2 specifically includes:

Step S2.1: determine whether the total length of the sequence output by the current inertial measurement unit is greater than or equal to the preset length N, and if so, go to step S2.2 to determine whether the pedestrian is in a stationary state;

Step S2.2: Calculate the modulus of the current three-axis gyroscope measurement vector ||y _k -b||, where y _k is the current three-axis gyroscope measurement vector, and b is the zero value of the three-axis gyroscope obtained in step S1 bias vector, and determine whether ||y _k -b|| is greater than the set first threshold th _m , if it is greater, then determine that the pedestrian is in a non-stationary state at the current moment, otherwise, go to step S2.3;

Step S2.3: further calculate the variance var of the vector mode measured by the gyroscope in the fixed-length window, and the calculation formula is shown in formula (2):

Among them, c represents the mean value of the vector moduli measured by N gyroscopes in the window. If var is greater than the set second threshold th _v , it is determined that the pedestrian is in a non-stationary state at the current moment, otherwise, it is determined that the pedestrian is in a stationary state at the current moment.

Specifically, please refer to FIG. 2 , which is a flowchart of an indoor pedestrian navigation pose estimation method in a specific example. The present invention detects the stationary state of pedestrians according to the output of the inertial sensor. In this embodiment, the current three-axis gyroscope output value amplitude and the gyroscope output value amplitude variance in the fixed-length window are used to detect the stationary state of pedestrians, and the window length is assumed to be N. If the total length of the current inertial sensor output sequence is less than N, and still detection cannot be performed, go to step S3 to execute. Otherwise, the judgment is made through steps S2.2 and S2.3.

In step S2, the first threshold th _m and the second threshold th _v are selected as follows: According to the static gyro data collected in step S1 for calibrating the zero bias error of the three-axis gyroscope, calculate all gyroscope measurements in the set of data the modulus of the vector, and find the maximum modulus value, taking 1.5 times of it as th _m ; starting from the Nth data, calculate the data composed of the modulus value of each gyroscope measurement vector and the previous N-1 gyroscope measurement vector modulus values The variance of the set, find the maximum variance, and use 1.5 times it as th _v .

In step S2, the setting of the window length N can be determined according to the stationary time length of a complete gait period in the normal walking process of the pedestrian. A large number of experimental test results by different scholars show that during normal walking, the resting time in a complete gait is about 0.3-0.5 seconds. Therefore, N can be set to 0.3F, where F represents the output frequency of the inertial sensor. The inventor of the present application found through a lot of practice and research that: N should not be set too large, as too large will lead to missed detection of the real stationary state; N is not easy to be set too small, too small, multiple stationary states will be detected in one gait, lead to increased computational overhead.

Of course, in the specific implementation, the detection of the static state can also be implemented in other manners, which is not particularly limited in the present invention.

Step S3: According to the initial attitude and the measurement of the three-axis gyroscope, the strapdown inertial navigation algorithm is used to update the attitude, and the current attitude is obtained recursively, wherein the measurement of the three-axis gyroscope is used as the sensor coordinate without considering the influence of the earth's rotation. The rotational angular velocity of the inertial measurement unit under the frame relative to the navigation coordinate system.

Specifically, according to the traditional strapdown inertial navigation algorithm, the current attitude is obtained recursively. In the recursive algorithm, the rotational angular velocity of the inertial measurement unit in the sensor coordinate system relative to the navigation coordinate system needs to be measured, and the output of the three-axis gyroscope in the inertial measurement unit is measured as the inertial measurement unit in the sensor coordinate system relative to the navigation coordinate system. , so the effect of the Earth's rotation needs to be deducted from it. Under the custom navigation coordinate system, it is very difficult to compensate the angular velocity of the earth's rotation for the output of the three-axis gyroscope. However, the angular velocity of the earth's rotation is very small, about 0.004 degrees per second, so it can be ignored. In the present invention, the measurement of the three-axis gyroscope is the rotational angular velocity of the inertial measurement unit in the sensor coordinate system relative to the navigation coordinate system.

In one embodiment, step S3 specifically includes:

According to the traditional strapdown inertial navigation algorithm, the current attitude is obtained recursively. The strapdown attitude update equation is as follows:

Among them, w(t) is the rotational angular velocity of the inertial measurement unit in the sensor coordinate system relative to the navigation coordinate system, q _k+1 represents the four elements of attitude at time t _k+1 , and q _k represents the four elements of attitude at time t _k .

Specifically, after the execution of the recursive posture update is completed, it is determined whether the navigation ends, and if so, the posture update is ended, otherwise, it returns to step S2.

Step S4: constructing a dynamic model according to the attitude update equation of the strapdown inertial navigation algorithm, constructing a measurement model based on the relationship between the output of the accelerometer and the gravity, and updating the current attitude through the Kalman filter under the quaternion frame.

Specifically, the pose update is performed by a Kalman filter under the quaternion framework. According to the strapdown inertial navigation algorithm, the current attitude can be deduced, but due to the sensor error, there is a large error in the attitude. The present invention uses the three-axis accelerometer in the stationary state of the pedestrian to construct the attitude observation to correct the current attitude.

Wherein, step S4 specifically includes:

Step S4.1: Establish the relationship between the real measurement vector of the current three-axis accelerometer in the sensor coordinate system (without considering the zero offset and other errors) and the attitude estimation,

表示当地重力在导航坐标系下投影的四元数形式，q_k+1为时间t_k+1时刻姿态四元素，

为时间t_k+1时刻传感器坐标系下三轴加速度计的真实测量向量的四元数形式；⊙为四元数乘法运算符；in,

represents the quaternion form of the projection of local gravity in the navigation coordinate system, q _k+1 is the four elements of attitude at time t _k+1 ,

is the quaternion form of the real measurement vector of the three-axis accelerometer in the sensor coordinate system at time tk ₊₁ ; ⊙ is the quaternion multiplication operator;

Step S4.2: Construct the dynamic model by the strapdown attitude update equation, construct the measurement model by the formula (4), and use the Kalman filter to estimate the current attitude information.

Specifically, please refer to FIG. 3 , which is a flowchart of the four-element Kalman filter. The current posture can be obtained recursively according to the formula in step S3. However, due to the sensor error, the attitude estimation error obtained in this way is relatively large, which needs to be corrected by using reliable external observations. When the pedestrian is stationary, the measurement of the three-axis accelerometer only reflects the influence of the earth's gravity (that is, the measurement value is equal to the projection of gravity in the current coordinate system), and the projection of gravity in the navigation coordinate system is known, so the current three-axis accelerometer can be established. Relationship between axis accelerometer measurements and attitude estimation.

为当地重力在导航坐标系下投影的四元数形式，根据导航坐标系的定义，重力在导航坐标系下的投影是已知的。三维向量通过补充一个0元素可转为四元数形式，如假设导航坐标系垂直地面向上，重力在导航坐标系下投影为gⁿ＝[0 0 -g]^T，改写成四元数形式为

室内导航应用中，不考虑导航区域内的重力变化，重力g为常数。

It is the quaternion form of the projection of the local gravity in the navigation coordinate system. According to the definition of the navigation coordinate system, the projection of the gravity in the navigation coordinate system is known. A three-dimensional vector can be converted into a quaternion form by adding a 0 element. For example, assuming that the navigation coordinate system is vertically upward, and the gravity is projected under the navigation coordinate system as g ⁿ = [0 0 -g] ^T , rewritten into the quaternion form as

In indoor navigation applications, the gravity change in the navigation area is not considered, and the gravity g is constant.

In one embodiment, step S4.2 specifically includes:

Step S4.2.1: Taking the attitude as the state variable, the following discrete four-element Kalman filter model is established according to equation (4) and the strapdown attitude update equation in step S3,

In formula (5), F _k+1 is the state transition matrix, H _k+1 is the state observation matrix, K _k+1 is the system noise coefficient matrix, W _k+1 is the observation noise coefficient matrix, δ _k+1 is the IMU The noise of the angular increment within the sampling interval, ε _k+1 represents the noise of the triaxial accelerometer measurement, which is specifically defined as follows:

为时间t_k+1时刻载体坐标系下三轴加速度计的输出向量；s_k+1和v_k+1分别是姿态四元素q_k+1的标量部分和矢量部分。In formula (6), Δθ _k+1 represents the angular increment vector measured by the three-axis gyroscope from time t _k to t _k+1 , Δθ _k+1 is the modulus of the vector Δθ _k+1 , and I _m represents m× The identity matrix of m; g ⁿ is the projection of the local gravity in the navigation coordinate system;

is the output vector of the three-axis accelerometer in the carrier coordinate system at time t _k+1 ; s _k+1 and v _k+1 are the scalar part and the vector part of the four-element attitude q _k+ 1 respectively.

Step S4.2.2: According to the discrete four-element Kalman filter model, the corrected attitude estimation at the current moment is obtained, in which, the state vector X _k+1 =q _k+1 in the model, in the filtering process, the co-ordination of noise δ _k+1 is The covariance matrix R _k+1 of the square matrix Q _k+1 and the noise ε _k+1 is calculated as follows:

In formula (7), σ ₁ represents the standard deviation of the angular increment error measured by the three-axis gyroscope within the sampling interval of the IMU, σ ₂ represents the standard deviation of the measurement noise of the three-axis accelerometer, M _k|k and M _{k+1 |k} is an intermediate variable, calculated as follows:

In formula (8), q _k|k is the last attitude estimation of the Kalman filter, q _k+1|k is the attitude prediction of the current model, P _k|k and P _k+1/k are q _k|k and q _k+1|k covariance matrix.

Specifically, the system model in the filter is a nonlinear equation, and it is very complicated to use the traditional Kalman filter for data fusion, so the present invention implements the Kalman filter under a four-element framework. Taking the four elements of attitude as the state quantity, a discrete four-element Kalman filter model is established. After the filtering is completed, go back to step S2.

Compared with the ordinary Kalman filter, the Kalman filter executed under the four-element framework in the present invention is different in that the two are executed under different frameworks, one is an ordinary three-dimensional vector framework, and the other is a four-element framework. , the algorithms under different frameworks are different, and the four-element Kalman filter can effectively avoid the complex linearization problem of the traditional Kalman filter.

In order to obtain accurate and reliable triaxial acceleration measurement, in one embodiment, the method further includes: performing error compensation on the triaxial accelerometer.

Specifically, the present invention takes into account the variation of the error of the three-axis accelerometer over time. In the specific implementation process, any feasible method can be used to compensate the error of the three-axis accelerometer, which is not particularly limited in the present invention. In this embodiment, the following compensation method is provided. The error effects of the three-axis accelerometer are unified into the zero bias error, and the zero bias error random walk model is established.

b _k+1 = b _k + η _k+1 (9)

In formula (9), b _k+1 and b _k represent the zero bias error of the three-axis accelerometer at the current moment and the previous moment, and η _k+1 is the current noise. Using the zero-velocity update method, taking the pedestrian's stationary speed as the observed quantity, and the pedestrian's velocity and the triaxial accelerometer deviation as the state quantities, the following discrete Kalman filter model can be established to estimate the zero-bias size of the triaxial accelerometer:

分别为系统噪声和观测噪声。A_k+1和B_k+1定义如下，In formula (10), x _k is the state vector composed of the pedestrian’s current speed and the three-axis accelerometer deviation, A _k+1 is the state transition matrix, B _k+1 is the state observation matrix, ω _k+1 and

are system noise and observation noise, respectively. A _k+1 and B _k+1 are defined as follows,

表示传感器坐标系到导航坐标系的方向余弦矩阵，I_3×1和0_3×1分别代表元素全为1的三维列向量和元素全为0的三维列向量。四元素卡尔曼滤波执行完毕，判断导航是否结束，如是则结束姿态更新，如否则返回到步骤S2。where T is the output time interval of the inertial sensor unit,

Represents the direction cosine matrix from the sensor coordinate system to the navigation coordinate system, I _3×1 and 0 _3×1 represent the three-dimensional column vector with all 1 elements and the three-dimensional column vector with all 0 elements, respectively. After the execution of the four-element Kalman filter is completed, it is judged whether the navigation ends, if so, the attitude update is ended, otherwise, return to step S2.

The above-mentioned 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 uses the measurement of the accelerometer at the static moment to construct the attitude estimation observation, and executes the Kalman filter under the four-element framework to perform data fusion to obtain accurate attitude information, thereby providing a simple and feasible method that does not depend on external attitude sensors. Pedestrian Navigation Pose Estimation Method.

Although preferred embodiments of the present invention have been described, additional changes and modifications to these embodiments may occur to those skilled in the art once the basic inventive concepts are known. Therefore, the appended claims are intended to be construed to include the preferred embodiment and all changes and modifications that fall within the scope of the present invention.

Obviously, those skilled in the art can make various changes and modifications to the embodiments of the present invention without departing from the spirit and scope of the embodiments of the present invention. Thus, provided that these 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 include such modifications and variations.

Claims (5)

1. the indoor pedestrian navigation attitude estimation method based on foot-mounted inertial measurement unit, is characterized in that, comprises: Step S1: Install the inertial measurement unit on the foot of the pedestrian, determine the navigation coordinate system, perform online calibration on the zero offset error of the inertial measurement unit, and initialize the initial attitude of the inertial measurement unit in the navigation coordinate system, wherein the Z of the navigation coordinate system is The axis is perpendicular to the ground; Step S2: determine whether the pedestrian is in a stationary state according to the output of the inertial sensor, if the pedestrian is in a non-stationary state, perform step S3, otherwise, perform step S4; Step S3: According to the initial attitude and the output of the three-axis gyroscope, the strapdown inertial navigation algorithm is used to update the attitude, and the current attitude is obtained by recursion, wherein the measurement of the three-axis gyroscope is used as the relative position of the inertial measurement unit in the sensor coordinate system. The rotational angular velocity of the navigation coordinate system; Step S4: constructing a dynamic model according to the attitude update equation of the strapdown inertial navigation algorithm, constructing a measurement model based on the relationship between accelerometer output and gravity, and updating the current attitude through a Kalman filter under the quaternion frame; Wherein, step S4 specifically includes: Step S4.1: Establish the relationship between the actual acceleration measurement vector and the attitude estimation in the current sensor coordinate system,

in,

represents the quaternion form of the projection of local gravity in the navigation coordinate system, q _k+1 is the attitude quaternion at time t _k+1 ,

is the quaternion form of the real measurement vector of the three-axis accelerometer in the sensor coordinate system at time tk ₊₁ ; ⊙ is the quaternion multiplication operator; Step S4.2: construct the dynamic model with the strapdown attitude update equation, construct the measurement model with the formula (4), and use the Kalman filter to estimate the current attitude information; Step S4.2 specifically includes: Step S4.2.1: take the attitude as the state variable, establish the following discrete quaternion Kalman filter model according to equation (4) and the strapdown attitude update equation in step S3,

In formula (5), F _k+1 is the state transition matrix, H _k+1 is the state observation matrix, K _k+1 is the system noise coefficient matrix, W _k+1 is the observation noise coefficient matrix, δ _k+1 is the IMU The noise of the angular increment within the sampling interval, ε _k+1 represents the noise of the triaxial accelerometer measurement, which is specifically defined as follows:

In formula (6), Δθ _k+1 represents the angular increment vector measured by the three-axis gyroscope from time t _k to t _k+1 , Δθ _k+1 is the modulus of the vector Δθ _k+1 , and I _m represents m× The identity matrix of m; g ⁿ is the projection of the local gravity in the navigation coordinate system;

is the output vector of the three-axis accelerometer in the carrier coordinate system at time t _k+1 ; s _k+1 and v _k+1 are the scalar part and the vector part of the attitude quaternion q _k+1 respectively; Step S4.2.2: According to the discrete quaternion Kalman filter model, the corrected attitude estimate at the current moment is obtained, wherein the state vector X _k+1 =q _k+1 in the model, during the filtering process, the noise δ _k+1 The covariance matrix R _k+ 1 of the covariance matrix Q _k+1 and the noise ε _k+1 is calculated as follows:

In formula (7), σ ₁ represents the standard deviation of the angular increment error measured by the three-axis gyroscope within the sampling interval of the inertial measurement unit, σ ₂ represents the standard deviation of the measurement noise of the three-axis accelerometer, M _k|k and M _{k +1|k} is an intermediate variable, calculated as follows:

In formula (8), q _k|k is the last attitude estimation of the Kalman filter, q _k+1|k is the attitude prediction of the current model, P _k|k and P _k+1/k are q _k|k and q _k+1|k covariance matrix. 2. The method of claim 1, wherein step S1 specifically comprises: Step S1.1: Install the inertial measurement unit on the foot of the pedestrian, customize the three-dimensional rectangular coordinate system as the navigation coordinate system, and ensure that the Z axis of the navigation coordinate system is perpendicular to the ground; Step S1.2: For a three-axis gyroscope, the average value of the output of each axis in a predetermined period of time in a static state is used as the zero-bias error of the axis; for a three-axis accelerometer, through four different positions in a static state The acceleration output within a preset time is calibrated, and the calibration formula is shown in formula (1),

Among them, b _x , b _y , b _z are the zero bias errors on the X, Y, and Z axes of the three-axis accelerometer, respectively, x _i , y _i , z _i , i=1, 2, 3, 4, representing the i-th accelerometer, respectively The average value of X, Y, Z outputs at each position, m _i , i=1, 2, 3, 4 represents the sum of the squares of the average output values of each axis at the ith position; Step S1.3: At the initial navigation moment, adjust the position of the inertial measurement unit to align the sensor coordinate system of the inertial measurement unit with the navigation coordinate system, so as to obtain the initial attitude of the inertial measurement unit in the navigation coordinate system, wherein the initial attitude is in the form of is q ₀ =[1,0,0,0] ^T . 3. The method of claim 1, wherein step S2 specifically comprises: Step S2.1: determine whether the total length of the sequence output by the current inertial measurement unit is greater than or equal to the preset length N, and if so, go to step S2.2 to determine whether the pedestrian is in a stationary state; Step S2.2: Calculate the modulus of the current three-axis gyroscope measurement vector ||y _k -b||, where y _k is the current three-axis gyroscope measurement vector, and b is the zero value of the three-axis gyroscope obtained in step S1 bias vector, and determine whether ||y _k -b|| is greater than the set first threshold th _m , if it is greater, then determine that the pedestrian is in a non-stationary state at the current moment, otherwise, go to step S2.3; Step S2.3: further calculate the variance var of the vector mode measured by the gyroscope in the fixed-length window, and the calculation formula is shown in formula (2):

Among them, c represents the mean value of the vector moduli measured by N gyroscopes in the window. If var is greater than the set second threshold th _v , it is determined that the pedestrian is in a non-stationary state at the current moment, otherwise, it is determined that the pedestrian is in a stationary state at the current moment. 4. The method of claim 1, wherein step S3 specifically comprises: According to the traditional strapdown inertial navigation algorithm, the current attitude is obtained recursively. The strapdown attitude update equation is as follows:

Among them, w(t) is the rotational angular velocity of the inertial measurement unit in the sensor coordinate system relative to the navigation coordinate system, q _k+1 represents the attitude quaternion at time t _k+1 , and q _k represents the attitude quaternion at time t _k . 5. The method according to any one of claims 1-4, wherein the method further comprises: Error compensation for three-axis accelerometers.

Images