CN110672095A - Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation - Google Patents

Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation Download PDF

Info

Publication number
CN110672095A
CN110672095A CN201910990940.2A CN201910990940A CN110672095A CN 110672095 A CN110672095 A CN 110672095A CN 201910990940 A CN201910990940 A CN 201910990940A CN 110672095 A CN110672095 A CN 110672095A
Authority
CN
China
Prior art keywords
matrix
error
ekf
acceleration
speed
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.)
Pending
Application number
CN201910990940.2A
Other languages
Chinese (zh)
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.)
Beijing Shaini Science And Technology Development Co Ltd
Original Assignee
Beijing Shaini Science And Technology Development Co Ltd
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 Beijing Shaini Science And Technology Development Co Ltd filed Critical Beijing Shaini Science And Technology Development Co Ltd
Priority to CN201910990940.2A priority Critical patent/CN110672095A/en
Publication of CN110672095A publication Critical patent/CN110672095A/en
Pending legal-status Critical Current

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention relates to the technical field of navigation positioning, and discloses a pedestrian indoor autonomous positioning algorithm based on micro inertial navigation, which comprises the following steps: a. step detection; b. calculating the attitude, position and speed by dead reckoning; c. and predicting and updating the system error based on the EKF frame. The problem of divergence of a PDR positioning algorithm is restrained, the MEMS and IMU modules are bound on the feet by utilizing the short static time of the feet when a person walks, the static interval of the feet when the person walks is judged by analyzing IMU data, the speed is returned to zero by utilizing the static interval to serve as a zero-speed measurement value, then an EKF algorithm is adopted, each error of an updating system is corrected by utilizing the zero-speed measurement value, and the divergence of the position of the system is restrained. The self-positioning method is realized under the environment (such as indoor environment) where GNSS is rejected, completely independent of external auxiliary equipment and only dependent on the wearable micro inertial navigation module, and the self-positioning of the pedestrian is realized.

Description

Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation
Technical Field
The invention relates to the technical field of navigation and positioning, in particular to a pedestrian indoor autonomous positioning algorithm based on micro inertial navigation.
Background
Along with the development of society, people have increasing demands on material culture and navigation positioning. In outdoor open environments, GNSS systems (global navigation satellite systems, such as GPS) are well established and are widely used in our daily lives; however, in the environments such as urban building groups, mountainous areas, forests, indoor areas, etc., GNSS systems often cannot receive signals, so that positioning becomes very difficult. Taking indoor pedestrian positioning as an example, many application scenes, such as battle, fire fighting, medical treatment, tour guide, shopping, games and the like, have urgent requirements on positioning and navigation, can provide better life experience for people, and is a powerful guarantee for life safety! Therefore, the research on the pedestrian positioning method which can be applied to the indoor has great practical significance and greatly promotes the development of the society.
Disclosure of Invention
The invention mainly provides a pedestrian indoor autonomous positioning algorithm based on micro inertial navigation, which is realized in a GNSS refusal environment, for example: in the indoor environment, do not rely on external auxiliary assembly at all, only rely on wearable little inertial navigation module, realize the problem of pedestrian's autonomic location.
In order to solve the technical problems, the invention adopts the following technical scheme:
a pedestrian indoor autonomous positioning algorithm based on micro inertial navigation comprises the following steps:
a. step detection;
b. calculating the attitude, position and speed by dead reckoning;
c. predicting and updating system errors based on an EKF frame;
2. the pedestrian indoor autonomous positioning algorithm based on micro inertial navigation according to claim 1, characterized in that:
the step a is based on the PDR positioning based on the ZUPT and the ZARU, the PDR positioning is based on the detection of the static moment of the IMU falling to the ground, and the detection of the IMU falling to the ground is judged by adopting three conditions based on an accelerometer and a gyroscope:
1: mode of acceleration
Figure BDA0002237373410000022
2: variance of acceleration mode over time
Figure BDA0002237373410000024
3: mode of angular velocity
Figure BDA0002237373410000026
Figure BDA0002237373410000027
Performing logical and operation on B1, B2 and B3, wherein B is B1& B2& B3, and if and only if B is 1, determining that the foot is grounded and stationary;
in the step b, because the MEMS has zero offset, the zero offset is removed from the original acceleration and angular velocity before the MEMS is input into the EKF frame:
Figure BDA0002237373410000031
wherein
Figure BDA0002237373410000032
The acceleration and the angular velocity of the carrier after zero offset correction at the moment k,
Figure BDA0002237373410000033
is an estimated value in an EKF model;
then, updating a direction cosine matrix, wherein the matrix is another expression form of the attitude and can be directly converted into an attitude Euler angle;
Figure BDA0002237373410000034
wherein
Figure BDA0002237373410000035
Is the DCM estimated by the carrier to navigation system at time k-1,
Figure BDA0002237373410000036
DCM, I predicted from the carrier to the navigation system at the time k3Is a 3 rd order identity matrix, δ ΩkIs thatΔ t is the IMU sampling time interval;
thus, the speed and position estimated under the navigation system can be obtained:
Figure BDA0002237373410000039
wherein v (k, k-1) and r (k, k-1) respectively represent the predicted speed and position at the moment k, are not corrected by EKF, and g represents the gravity acceleration, i.e. g is approximately equal to 9.8m/s2；
Then, according to the error of EKF estimation, the speed, position and DCM matrix are corrected:
Figure BDA0002237373410000041
wherein r (k, k), v (k, k),
Figure BDA0002237373410000042
respectively representing the k time, and correcting the position, the speed and the DCM matrix after the EKF is statically detected according to the previous step; delta rkδ v is an EKF state estimation error; delta thetakIs about the state estimation error delta phikThe antisymmetric matrix of (a):
Figure BDA0002237373410000043
in the step c, the error state of the EKF frame comprises 15 elements:
Figure BDA0002237373410000044
here, the δ X (k, k) state is, in turn, represented as δ φk: pitch, roll, yaw angle errors;
Figure BDA0002237373410000045
carrier three-axis angular velocity error; delta rk: a spatial position error; delta vk: a spatial velocity error;
Figure BDA0002237373410000046
the carrier triaxial acceleration error, so the state transition model is:
δX(k，k-1)＝ΦkδX(k-1，k-1)+Qk-1(1.15)
wherein the state transition matrix is:
Figure BDA0002237373410000047
wherein the content of the first and second substances,
Figure BDA0002237373410000048
is acceleration under navigation system
Figure BDA0002237373410000049
Of an inverse-symmetric matrix, Qk-1Is process noise;
Figure BDA0002237373410000051
Figure BDA0002237373410000052
the state covariance prediction model is:
Figure BDA0002237373410000053
the measurement model is as follows:
Zk＝HδX(k，k)+Rk(1.20)
Zkfor the measured values, H is the measurement matrix, RkTo measure noise;
thus, the measurement update at time k can be expressed as:
δX(k，k)＝δX(k-1，k-1)+Kk[Zk-HδX(k，k-1)](1.21)
wherein, KkExpressed as kalman gain:
Figure BDA0002237373410000054
then, updating the state covariance to complete the EKF iteration process;
Figure BDA0002237373410000055
in practical use, the measured values are the velocity v (k, k-1) and ω (k, k-1) when the foot is down, and the corresponding measured value and observation matrix is:
in order to further improve the precision, in the invention, the error between the altitude information measured by the barometer and the vertical displacement calculated by dead reckoning is utilized, the output course of the magnetometer and the course error calculated by an algorithm are introduced, the ZUPT and the ZARU are jointly used as the measured values, the state is cooperatively constrained, the precision is improved, and the corresponding measured values and the observation matrix are as follows:
Figure BDA0002237373410000062
wherein Δ yaw ═ δ Φ (3) - ΦMag，Δh＝r(3)-Hbarometer。
Further, still include:
acquiring an initial pose:
[1] calculating an initial position and an attitude, setting the initial position to be zero, and calculating the initial attitude by (1.26):
Figure BDA0002237373410000063
where pitch represents the initial pitch angle, roll represents the initial roll angle, G represents the gravitational acceleration, ax，ayRespectively representing the acceleration output values of the x axis and the y axis of the machine body;
[2]and calculating the course by the magnetometer: first according to [1]The pitch and roll angles are calculated in the step (2), and the magnetometer is output to a data body coordinate system: mB_MAG(mx，my，mz) Is converted into a navigation coordinate system by the formula (1.27) and is marked as MN_MAG(Bx，By，Bz) Then, the heading is calculated according to the formula (1.28), and the tilt compensation is realized:
Figure BDA0002237373410000071
Figure BDA0002237373410000072
[3] converting the pitch angle, the roll angle and the yaw angle obtained in the previous step into a DCM matrix as initial values:
Figure BDA0002237373410000073
wherein p represents pitch, r represents roll, and φ represents φmag_yaw。
Has the advantages that: the problem of divergence of a PDR positioning algorithm is restrained, the MEMS and IMU modules are bound on the feet by utilizing the short static time of the feet when a person walks, the static interval of the feet when the person walks is judged by analyzing IMU data, the speed is returned to zero by utilizing the static interval to serve as a zero-speed measurement value, then an EKF algorithm is adopted, each error of an updating system is corrected by utilizing the zero-speed measurement value, and the divergence of the position of the system is restrained. The self-positioning method is realized under the environment (such as indoor environment) where GNSS is rejected, completely independent of external auxiliary equipment and only dependent on the wearable micro inertial navigation module, and the self-positioning of the pedestrian is realized.
Drawings
FIG. 1 is a flow chart of an embodiment positioning algorithm;
FIG. 2 is a diagram illustrating a test result of an indoor simple environment algorithm according to an embodiment;
FIG. 3 is a diagram illustrating the results of an indoor EMI environment algorithm test according to an embodiment;
FIG. 4 is a diagram illustrating the test results of an indoor long-time testing algorithm according to an embodiment;
FIG. 5 is a graph of the test results of an indoor and outdoor combined wide-range long-time test algorithm according to an embodiment;
Detailed Description
The following will further describe in detail a pedestrian indoor autonomous positioning algorithm technical solution based on micro inertial navigation according to an embodiment of the present invention.
It should be noted that the test methods described in the following embodiments, unless otherwise specified, are all conventional methods, and are commercially available; in the description of the present invention, the terms "lateral", "longitudinal", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc., indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings, are only for convenience in describing the present invention and simplifying the description, and do not indicate or imply that the device or element being referred to must have a particular orientation, be constructed and operated in a particular orientation, and thus, should not be construed as limiting the present invention.
The first embodiment is as follows: as shown in fig. 1 and fig. 2, the pedestrian indoor autonomous positioning algorithm based on micro inertial navigation of the present embodiment includes the following steps:
a. step detection;
b. calculating the attitude, position and speed by dead reckoning;
c. predicting and updating system errors based on an EKF frame;
2. the pedestrian indoor autonomous positioning algorithm based on micro inertial navigation according to claim 1, characterized in that:
the step a is based on the PDR positioning based on the ZUPT and the ZARU, the PDR positioning is based on the detection of the static moment of the IMU falling to the ground, and the detection of the IMU falling to the ground is judged by adopting three conditions based on an accelerometer and a gyroscope:
1: mode of acceleration
Figure BDA0002237373410000081
2: variance of acceleration mode over time
Figure BDA0002237373410000091
Figure BDA0002237373410000092
Figure BDA0002237373410000093
3: mode of angular velocity
Figure BDA0002237373410000094
Figure BDA0002237373410000095
Performing logical and operation on B1, B2 and B3, wherein B is B1& B2& B3, and if and only if B is 1, determining that the foot is grounded and stationary;
in the step b, because the MEMS has zero offset, the zero offset is removed from the original acceleration and angular velocity before the MEMS is input into the EKF frame:
Figure BDA0002237373410000096
wherein
Figure BDA0002237373410000097
The acceleration and the angular velocity of the carrier after zero offset correction at the moment k,
Figure BDA0002237373410000098
is an estimated value in an EKF model;
then, updating a direction cosine matrix, wherein the matrix is another expression form of the attitude and can be directly converted into an attitude Euler angle;
Figure BDA0002237373410000099
wherein
Figure BDA00022373734100000910
Is the DCM estimated by the carrier to navigation system at time k-1,
Figure BDA00022373734100000911
DCM, I predicted from the carrier to the navigation system at the time k3Is a 3 rd order identity matrix, δ ΩkIs that
Figure BDA00022373734100000912
Δ t is the IMU sampling time interval;
Figure BDA0002237373410000101
thus, the speed and position estimated under the navigation system can be obtained:
Figure BDA0002237373410000102
wherein v (k, k-1) and r (k, k-1) respectively represent the predicted speed and position at the moment k, are not corrected by EKF, and g represents the gravity acceleration, i.e. g is approximately equal to 9.8m/s2；
Then, according to the error of EKF estimation, the speed, position and DCM matrix are corrected:
Figure BDA0002237373410000103
wherein r (k, k), v (k, k),
Figure BDA0002237373410000104
respectively representing the k time, and correcting the position, the speed and the DCM matrix after the EKF is statically detected according to the previous step; delta rkδ v is an EKF state estimation error; delta thetakIs about the state estimation error delta phikThe antisymmetric matrix of (a):
Figure BDA0002237373410000105
in the step c, the error state of the EKF frame comprises 15 elements:
Figure BDA0002237373410000106
here, the δ X (k, k) state is, in turn, represented as δ φk: pitch, roll, yaw angle errors;
Figure BDA0002237373410000111
carrier three-axis angular velocity error; delta rk: a spatial position error; delta vk: a spatial velocity error;
Figure BDA0002237373410000112
the carrier triaxial acceleration error, so the state transition model is:
δX(k，k-1)＝ΦkδX(k-1，k-1)+Qk-1(1.15)
wherein the state transition matrix is:
Figure BDA0002237373410000113
wherein the content of the first and second substances,
Figure BDA0002237373410000114
is acceleration under navigation system
Figure BDA0002237373410000115
Of an inverse-symmetric matrix, Qk-1Is process noise;
Figure BDA0002237373410000116
Figure BDA0002237373410000117
the state covariance prediction model is:
Figure BDA0002237373410000118
the measurement model is as follows:
Zk＝HδX(k，k)+Rk(1.20)
Zkfor the measured values, H is the measurement matrix, RkTo measure noise;
thus, the measurement update at time k can be expressed as:
δX(k，k)＝δX(k-1，k-1)+Kk[Zk-HδX(k，k-1)](1.21)
wherein, KkExpressed as kalman gain:
Figure BDA0002237373410000121
then, updating the state covariance to complete the EKF iteration process;
Figure BDA0002237373410000122
in practical use, the measured values are the velocity v (k, k-1) and ω (k, k-1) when the foot is down, and the corresponding measured value and observation matrix is:
Figure BDA0002237373410000123
in order to further improve the precision, in the invention, the error between the altitude information measured by the barometer and the vertical displacement calculated by dead reckoning is utilized, the output course of the magnetometer and the course error calculated by an algorithm are introduced, the ZUPT and the ZARU are jointly used as the measured values, the state is cooperatively constrained, the precision is improved, and the corresponding measured values and the observation matrix are as follows:
wherein Δ yaw ═ δ Φ (3) - ΦMag，Δh＝r(3)-Hbarometer。
Further comprising: acquiring an initial pose:
[1] calculating an initial position and an attitude, setting the initial position to be zero, and calculating the initial attitude by (1.26):
Figure BDA0002237373410000131
where pitch represents the initial pitch angle, roll represents the initial roll angle, G represents the gravitational acceleration, ax，ayRespectively representing the acceleration output values of the x axis and the y axis of the machine body;
[2]and calculating the course by the magnetometer: first according to [1]The pitch and roll angles are calculated in the step (2), and the magnetometer is output to a data body coordinate system: mB_MAG(mx，my，mz) Is converted into a navigation coordinate system by the formula (1.27) and is marked as MN_MAG(Bx，By，Bz) Then, the heading is calculated according to the formula (1.28), and the tilt compensation is realized:
Figure BDA0002237373410000132
Figure BDA0002237373410000133
[3] converting the pitch angle, the roll angle and the yaw angle obtained in the previous step into a DCM matrix as initial values:
Figure BDA0002237373410000134
wherein p represents pitch, r represents roll, and φ represents φmag_yaw。
Further, the algorithm operates as follows:
[1] IMU data filtering: in order to improve the resolving precision, firstly, preprocessing acceleration and gyroscope original data, wherein the acceleration and gyroscope original data comprise acceleration low-pass filtering and gyroscope band-pass filtering, and an adopted filter is Butterworth second-order filtering;
[2] removing zero offset by combining the acceleration and gyroscope data with the state error estimated by the EKF according to the formula (1.8);
[3]from the gyroscope data and the DCM matrix at the previous time according to (1.9): cDCMUpdate the current time CDCMA matrix;
[4]from the obtained CDCMMatrix, converting the body acceleration into the acceleration under the navigation system by the formula (1.17)
Figure BDA0002237373410000141
[5]Is composed of [3]Calculated CDCMMatrix and [ 4]]Calculating the estimated speed and position of the obtained navigation acceleration according to the formula (1.11) by using the dead reckoning principle;
[6] calculating a state transition matrix from (1.16);
[7]determining the acceleration of navigation system from (1.18)
Figure BDA0002237373410000142
An antisymmetric matrix of (a);
[8] calculating a process noise matrix according to the time parameters;
[9] calculating a state covariance prediction model according to (1.19);
[10] and judging whether the step is in a zero-speed interval, wherein 1 represents a static interval, and 0 represents a swing interval according to the formulas (1.1) to (1.7), so that the step detection is realized.
If the judgment result is '1' according to the [10], the following operations are executed:
[11] updating the kalman gain according to equation (1.22);
[12] determining a measurement value according to equation (1.25), and measuring a transfer matrix;
[13] and updating the state quantity according to the formula (1.21) on the basis of the formulas (11) and (12);
[14] updating the state covariance matrix according to the formula (1.22) on the basis of the (11) and the (12);
[15] updating the cosine matrix of the position, the speed and the direction according to the formula (1.12) according to the state quantity updated in the step (13);
[16] storing the cosine matrix of the position, the speed and the direction in the step (15) as an initial value of the next moment and a starting point of the dead reckoning of the next moment;
[17] and if the judgment result is '0' according to the step [10], skipping the steps from the step [11] to the step [15], and directly executing the step [16 ].
The algorithm firstly carries out pedestrian 'step detection' according to sensor data of a micro inertial navigation module and in combination with a human kinematics rule, and identifies a 'zero-speed interval' (a static interval) in the moving process of a pedestrian; then, the state output of the static interval is utilized, and the linear velocity and the angular velocity in the interval are artificially determined to be zero and are taken as a zero-velocity measurement value; and finally, an EKF algorithm framework is adopted, the zero-speed measurement value, magnetometer data and barometer data are jointly used as an observed value of an EKF state, errors of position, speed and attitude in the course of dead reckoning are corrected, and the purpose of accurate positioning is achieved.
As shown in fig. 1, wherein the barometer, magnetometer, gyroscope, accelerometer in the box represent the sensors used; the content of the dashed box represents the measurement values under the EKF framework for correcting the systematic errors.
In the following, the actual meanings represented by the various parts in fig. 1 are briefly described:
initial pose: the method comprises the steps of initializing state variables, setting initial positions and calculating initial postures and direction cosine matrixes of carriers.
Attitude and position estimation: after zero offset of the body acceleration is removed, the body acceleration is rotated to a navigation coordinate system through a direction cosine matrix, and linear acceleration is obtained after gravity is removed; then integrating to obtain the speed, and integrating the speed again to obtain the position; because the position and the speed through integration have errors, the system state errors are measured through an EKF frame to correct the position, the speed and the attitude errors. And finally outputting the corrected position, speed and attitude information.
Step detection: mainly tie up the orientation module on the foot, according to gyroscope and acceleration data, judge "stationary region" when pedestrian's foot lands and realize the step and detect. In the "rest interval", 2 results were obtained, i.e., ZUPT (zero velocity interval update, whose measured value is the difference between the output velocity and zero) and ZARU (zero angular velocity interval update, whose measured value is the gyroscope output), as measured values of the EKF frame.
EKF state prediction and update: in attitude and position estimation, velocity and position are obtained by acceleration integration, and direction cosine matrix is obtained by gyroscope integration, thus obtaining a prediction equation of the system, and then according to sensor measurement values, such as barometer height difference, magnetometer calculated angle difference, ZARU and ZUPT. And updating the system state error.
Zero offset correction: and correcting the zero offset error of the accelerometer and the gyroscope by using the acceleration and the angular velocity offset estimated in the EKF system state error, so that the measurement result is more accurate.
In order to verify the accuracy of the algorithm, the test is carried out in a plurality of indoor and outdoor scenes.
The evaluation method comprises the following steps:
first, the error between the Start point (Start mark in fig. 2 to 5) and the End point (End mark in fig. 2 to 5) of the observation positioning (in the actual trajectory of the motion, the Start point of the pedestrian coincides with the End point).
And secondly, testing whether the repeatability of the positioning track is good (in actual walking, more repeated tracks exist, and the contact ratio of the middle positioning position is good or not in the movement process is mainly verified).
In fig. 2, in a simple indoor environment (less magnetic field interference), the accuracy of rectangular motion in a small range is tested, and the rectangular track is repeated 4 times, so that the track coincidence degree is very good, and the starting point and the end point are basically coincident.
In fig. 3, in an indoor complex environment (with large magnetic field interference), the contact ratio when the track moves along the fixed track is tested, in an actual test, the track moves repeatedly 8 times, the contact ratio is also very good, and the starting point and the end point are basically overlapped.
In fig. 4, the accuracy of long-time positioning is tested, the walking distance is greater than 1000 meters, the time is about 25 minutes, and it can be seen that the overlap ratio of the algorithm track is very good, and the error of the starting point and the end point is less than 1 meter.
In fig. 5, the limit test is performed, the pedestrian moves about 4000 meters in combination indoors and outdoors, the test time is about 55 minutes, the moving range is large (a rectangular area with the length of about 180 meters and the width of about 120 meters), and the starting point and the end point are about 7 meters in error from the test result.
From the experimental results of fig. 2 to 5, it can be concluded that the present invention has very high positioning accuracy and very good track contact ratio; under the condition of independent positioning without depending on external equipment, the algorithm has low divergence speed and high robustness. Multiple experiments prove that the algorithm precision is superior to 0.2 percent, exceeds similar positioning schemes on the market and reaches the domestic advanced level.
Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that changes, modifications, substitutions and alterations can be made in these embodiments without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.

Claims (3)

1. The pedestrian indoor autonomous positioning algorithm based on micro inertial navigation is characterized in that: the method comprises the following steps:
a. step detection;
b. calculating the attitude, position and speed by dead reckoning;
c. and predicting and updating the system error based on the EKF frame.
2. The pedestrian indoor autonomous positioning algorithm based on micro inertial navigation according to claim 1, characterized in that:
step a, PDR positioning based on ZUPT and ZARU, wherein the PDR positioning is based on detecting the static moment of the I steel landing, and the landing of the IMU is detected based on the judgment of an accelerometer and a gyroscope by adopting three conditions:
1: mode of acceleration
Figure FDA0002237373400000011
Figure FDA0002237373400000012
2: variance of acceleration mode over time
Figure FDA0002237373400000015
3: mode of angular velocity
Figure FDA0002237373400000016
Figure FDA0002237373400000021
Performing logical and operation on B1, B2 and B3, wherein B is B1& B2& B3, and if and only if B is 1, determining that the foot is grounded and stationary;
in the step b, because the MEMS has zero offset, the zero offset is removed from the original acceleration and angular velocity before the MEMS is input into the EKF frame:
wherein
Figure FDA0002237373400000023
The acceleration and the angular velocity of the carrier after zero offset correction at the moment k,
Figure FDA0002237373400000024
is an estimated value in an EKF model;
then, updating a direction cosine matrix, wherein the matrix is another expression form of the attitude and can be directly converted into an attitude Euler angle;
whereinIs the DCM estimated by the carrier to navigation system at time k-1,
Figure FDA0002237373400000027
DCM, I predicted from the carrier to the navigation system at the time k3Is a 3 rd order identity matrix, δ ΩkIs that
Figure FDA0002237373400000028
Δ t is the IMU sampling time interval;
Figure FDA0002237373400000029
thus, the speed and position estimated under the navigation system can be obtained:
Figure FDA00022373734000000210
wherein v (k, k-1) and r (k, k-1) respectively represent the predicted speed and position at the moment k, are not corrected by EKF, and g represents the gravity acceleration, i.e. g is approximately equal to 9.8m/s2；
Then, according to the error of EKF estimation, the speed, position and DCM matrix are corrected:
Figure FDA0002237373400000031
wherein r (k, k), v (k, k),
Figure FDA0002237373400000032
respectively representing the k time, and correcting the position, the speed and the DCM matrix after the EKF is statically detected according to the previous step; delta rkδ v is an EKF state estimation error; delta thetakIs about the state estimation error delta phikThe antisymmetric matrix of (a):
Figure FDA0002237373400000033
in the step c, the error state of the EKF frame comprises 15 elements:
Figure FDA0002237373400000034
here, the δ X (k, k) state is, in turn, represented as δ φk: pitch, roll, yaw angle errors;carrier three-axis angular velocity error; delta rk: a spatial position error; delta vk: a spatial velocity error;
Figure FDA0002237373400000036
the carrier triaxial acceleration error, so the state transition model is:
δX(k，k-1)＝ΦkδX(k-1，k-1)+Qk-1(1.15)
wherein the state transition matrix is:
wherein the content of the first and second substances,
Figure FDA0002237373400000042
is acceleration under navigation system
Figure FDA0002237373400000043
Of an inverse-symmetric matrix, Qk-1Is process noise;
Figure FDA0002237373400000044
Figure FDA0002237373400000045
the state covariance prediction model is:
Figure FDA0002237373400000046
the measurement model is as follows:
Zk＝HδX(k，k)+Rk(1.20)
Zkfor the measured values, H is the measurement matrix, RkTo measure noise;
thus, the measurement update at time k can be expressed as:
δX(k，k)＝δX(k-1，k-1)+Kk[Zk-HδX(k，k-1)](1.21)
wherein, KkExpressed as kalman gain:
Figure FDA0002237373400000047
then, updating the state covariance to complete the EKF iteration process;
Figure FDA0002237373400000048
in practical use, the measured values are the velocity v (k, k-1) and ω (k, k-1) when the foot is down, and the corresponding measured value and observation matrix is:
Figure FDA0002237373400000051
in order to further improve the precision, in the invention, the error between the altitude information measured by the barometer and the vertical displacement calculated by dead reckoning is utilized, the output course of the magnetometer and the course error calculated by an algorithm are introduced, the ZUPT and the ZARU are jointly used as the measured values, the state is cooperatively constrained, the precision is improved, and the corresponding measured values and the observation matrix are as follows:
Figure FDA0002237373400000052
wherein Δ yaw ═ δ Φ (3) - ΦMag，Δh＝r(3)-Hbarometer。
3. The pedestrian indoor autonomous positioning algorithm based on micro inertial navigation according to claim 2, characterized in that: further comprising:
acquiring an initial pose:
[1] calculating an initial position and an attitude, setting the initial position to be zero, and calculating the initial attitude by (1.26):
Figure FDA0002237373400000053
where pitch represents the initial pitch angle, roll represents the initial roll angle, G represents the gravitational acceleration, ax，ayRespectively representing the acceleration output values of the x axis and the y axis of the machine body;
[2]and calculating the course by the magnetometer: first according to [1]The pitch and roll angles are calculated in the step (2), and the magnetometer is output to a data body coordinate system: mB_MAG(mx，my，mz) Is converted into a navigation coordinate system by the formula (1.27) and is marked as MN_MAG(Bx，By，Bz) Then, the heading is calculated according to the formula (1.28), and the tilt compensation is realized:
Figure FDA0002237373400000061
Figure FDA0002237373400000062
[3] converting the pitch angle, the roll angle and the yaw angle obtained in the previous step into a DCM matrix as initial values:
Figure FDA0002237373400000063
wherein p represents pitch, r represents roll, and φ represents φmag_yaw。
CN201910990940.2A 2019-10-17 2019-10-17 Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation Pending CN110672095A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910990940.2A CN110672095A (en) 2019-10-17 2019-10-17 Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910990940.2A CN110672095A (en) 2019-10-17 2019-10-17 Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation

Publications (1)

Publication Number Publication Date
CN110672095A true CN110672095A (en) 2020-01-10

Family

ID=69082983

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910990940.2A Pending CN110672095A (en) 2019-10-17 2019-10-17 Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation

Country Status (1)

Country Link
CN (1) CN110672095A (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111637887A (en) * 2020-06-01 2020-09-08 太原理工大学 Mining monorail crane positioning method based on inertia module
CN112066980A (en) * 2020-08-31 2020-12-11 南京航空航天大学 Pedestrian navigation positioning method based on human body four-node motion constraint
CN113092819A (en) * 2021-04-14 2021-07-09 东方红卫星移动通信有限公司 Dynamic zero-speed calibration method and system for foot accelerometer
CN113390416A (en) * 2021-06-15 2021-09-14 浙江奥新智能科技有限公司 Single-base-station indoor positioning system and positioning method

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2657647A1 (en) * 2012-04-23 2013-10-30 Deutsches Zentrum für Luft- und Raumfahrt e. V. Method for estimating the position and orientation using an inertial measurement unit fixed to a moving pedestrian
CN103968827A (en) * 2014-04-09 2014-08-06 北京信息科技大学 Wearable human body gait detection self-localization method
CN105043385A (en) * 2015-06-05 2015-11-11 北京信息科技大学 Self-adaption Kalman filtering method for autonomous navigation positioning of pedestrians
CN107289933A (en) * 2017-06-28 2017-10-24 东南大学 Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions
CN109827577A (en) * 2019-03-26 2019-05-31 电子科技大学 High-precision inertial navigation location algorithm based on motion state detection

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2657647A1 (en) * 2012-04-23 2013-10-30 Deutsches Zentrum für Luft- und Raumfahrt e. V. Method for estimating the position and orientation using an inertial measurement unit fixed to a moving pedestrian
CN103968827A (en) * 2014-04-09 2014-08-06 北京信息科技大学 Wearable human body gait detection self-localization method
CN105043385A (en) * 2015-06-05 2015-11-11 北京信息科技大学 Self-adaption Kalman filtering method for autonomous navigation positioning of pedestrians
CN107289933A (en) * 2017-06-28 2017-10-24 东南大学 Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions
CN109827577A (en) * 2019-03-26 2019-05-31 电子科技大学 High-precision inertial navigation location algorithm based on motion state detection

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
漆钰晖 等: "多传感器行人航位推算方法和UKF融合算法", 《测绘通报》, no. 3, pages 49 - 54 *
闫双建: "基于多传感器零速修正的行人导航定位系统研究", 《中国优秀硕士学位论文全文数据库信息科技辑》, pages 140 - 259 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111637887A (en) * 2020-06-01 2020-09-08 太原理工大学 Mining monorail crane positioning method based on inertia module
CN112066980A (en) * 2020-08-31 2020-12-11 南京航空航天大学 Pedestrian navigation positioning method based on human body four-node motion constraint
CN113092819A (en) * 2021-04-14 2021-07-09 东方红卫星移动通信有限公司 Dynamic zero-speed calibration method and system for foot accelerometer
CN113390416A (en) * 2021-06-15 2021-09-14 浙江奥新智能科技有限公司 Single-base-station indoor positioning system and positioning method

Similar Documents

Publication Publication Date Title
CN109827577B (en) High-precision inertial navigation positioning algorithm based on motion state detection
Bai et al. A high-precision and low-cost IMU-based indoor pedestrian positioning technique
CN110398245B (en) Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit
CN110672095A (en) Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN107490378B (en) Indoor positioning and navigation method based on MPU6050 and smart phone
CN101726295B (en) Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation
CN103776446B (en) A kind of pedestrian's independent navigation computation based on double MEMS-IMU
CN104713554A (en) Indoor positioning method based on MEMS insert device and android smart mobile phone fusion
CN106168485B (en) Walking track data projectional technique and device
WO2016198009A1 (en) Heading checking method and apparatus
CN111721288B (en) Zero offset correction method and device for MEMS device and storage medium
CN106662443A (en) Methods and systems for vertical trajectory determination
CN111024075B (en) Pedestrian navigation error correction filtering method combining Bluetooth beacon and map
CN105509739A (en) Tightly coupled INS/UWB integrated navigation system and method adopting fixed-interval CRTS smoothing
CN109540135B (en) Method and device for detecting pose and extracting yaw angle of paddy field tractor
CN108957510B (en) Pedestrian seamless integrated navigation positioning method based on inertia/zero speed/GPS
CN107255474B (en) PDR course angle determination method integrating electronic compass and gyroscope
CN108007477B (en) Inertial pedestrian positioning system error suppression method based on forward and reverse filtering
CN106370178B (en) Attitude measurement method and device of mobile terminal equipment
CN108132053B (en) Pedestrian track construction method and system and inertia measurement device
CN111024070A (en) Inertial foot binding type pedestrian positioning method based on course self-observation
KR20130059344A (en) Method and system for detection of a zero velocity state of an object
Woyano et al. Evaluation and comparison of performance analysis of indoor inertial navigation system based on foot mounted IMU
CN103674064B (en) Initial calibration method of strapdown inertial navigation system

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