CN117268377A - Pedestrian navigation method and system based on K-step differential motion detection and track heading - Google Patents

Pedestrian navigation method and system based on K-step differential motion detection and track heading Download PDF

Info

Publication number
CN117268377A
CN117268377A CN202311138383.4A CN202311138383A CN117268377A CN 117268377 A CN117268377 A CN 117268377A CN 202311138383 A CN202311138383 A CN 202311138383A CN 117268377 A CN117268377 A CN 117268377A
Authority
CN
China
Prior art keywords
heading
track
pedestrian
course
error
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
CN202311138383.4A
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.)
Chongqing University of Post and Telecommunications
Original Assignee
Chongqing University of Post and Telecommunications
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 Chongqing University of Post and Telecommunications filed Critical Chongqing University of Post and Telecommunications
Priority to CN202311138383.4A priority Critical patent/CN117268377A/en
Publication of CN117268377A publication Critical patent/CN117268377A/en
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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

Abstract

The invention discloses a pedestrian navigation method and system based on K-step differential motion detection and track heading, and belongs to the technical field of navigation positioning. Secondly, a track course-position observation model based on the track course is established by calculating a deviation angle between the track course of the path extracted by the linear motion detection method and the step course of the pedestrian, and the track deviation is restrained by a Kalman filter. And finally outputting the corrected position, speed and posture results.

Description

Pedestrian navigation method and system based on K-step differential motion detection and track heading
Technical Field
The invention belongs to the technical field of navigation and positioning, and relates to a pedestrian navigation algorithm and system based on K-step differential motion detection and track heading assistance.
Background
A global satellite navigation positioning system (GNSS) is one of the most widely used navigation positioning systems at present, but in places such as underground mines, large buildings, tunnels, etc., navigation positioning services cannot be provided for pedestrians due to GNSS signal rejection. Inertial navigation positioning systems based on micro-electromechanical systems (MEMS) can provide autonomous navigation positioning services for pedestrians, but are limited by the performance of MEMS inertial sensors, and have low navigation positioning accuracy for a long time.
Many researchers have used methods combined with other sensor sources to improve the accuracy of personnel positioning systems, and literature (YU D, LI C, xia J. Neural Networks-Based Wi-Fi/PDR Indoor Navigation Fusion Methods [ J ]. IEEE Transactions on Instrumentation and Measurement,2023, 72:1-14.) has studied a technique for fusing MEMS inertial navigation with Wi-Fi fingerprint positioning using LSTM neural Networks to improve error accumulation for inertial navigation. The combined system of UWB and IMU is studied in literature (WANG M, PAN X, AN L, et al, AN Optimal Cooperative Navigation Algorithm based on Factor Graph for Pedestrians [ Z ].2021 3rd International Conference on Intelligent Control,Measurement and Signal Processing and Intelligent Oil Field (ICMSP). 2021:51-6.10.1109/ICMSP 53480.2021.953365), and the positioning accuracy of inertial navigation is improved through UWB ranging information provided indoors. The literature (CHEN J, ZHOU B, BAO S, et al A Data-Driven Inertial Navigation/Bluetooth Fusion Algorithm for Indoor Localization [ J ]. IEEE Sensors Journal,2022,22 (6): 5288-301) combines MEMS inertial navigation with the positioning of Bluetooth Low Energy (BLE) via a particle filter to improve the positioning accuracy of inertial navigation. The above method, although improving the positioning accuracy of the inertial navigation algorithm, needs to provide corresponding infrastructure, and increases the cost of the navigation device and the complexity of the system. On the other hand, some researchers introduce human motion constraints and scene constraints to suppress errors in inertial navigation systems. Literature (jirnez a R, SECO F, prito J C, et al, standing pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU; proceedings of the 2010 7th workshop on positioning,navigation and communication,F,2010[C, ieee.) uses zero speed correction system navigation errors during the standing phase of pedestrians, but zero speed update (ZUPT) cannot observe heading errors, resulting in ineffective suppression of heading errors. Document (RAJAGOPAL S J M SD P, STOCKHOLM, SWEDEN.Personal dead reckoning system with shoe mounted inertial sensors [ J ]. 2008.) uses the zero angular rate during the standing phase to observe the attitude error of the system, but during the standing phase, the quasi-static state of the gyro cannot be accurately obtained, so the correction of the attitude by the method is always limited.
The classical heuristic course drift suppression (HDE) method fully utilizes the characteristics of the building and utilizes the direction of the building to suppress the course error accumulation of the inertial navigation system. The improved heuristic heading drift suppression (iHDE) approach allows for more complex scenarios in buildings, such as 45 ° angled aisles, curved aisles, and wide areas suitable for non-directional movement. The iHDE method presets eight dominant directions to constrain error accumulation of the inertial navigation system. In comparison to the HDE method, the iHDE method performs better in non-ideal trajectories than the HDE method. However, both the HDE method and the iHDE method are strapdown inertial navigation algorithm (INS) inertial navigation methods for restraining feet by using a set dominant direction, and in an actual motion process, the INS heading is not consistent with a building direction, so that the use of the building direction to restrain the INS heading may cause upward deviation of the INS heading, and in addition, when a pedestrian walks in a set dominant direction, the iHDE method is degenerated into an ins+extended kalman filter (EKF) +zupt method, and the heading error cannot be effectively restrained.
The precondition of the use of the HDE method and the iHDE method is that the straight-line path in the walking path is judged, and then the heading error accumulation problem of the straight-line path is restrained by utilizing the pre-defined dominant direction. The traditional motion detection method is to calculate the difference between the current heading and the average heading in the heading of the window, and take the maximum difference value as the detection amount of the linear motion. On a slow turning path, the course change between the two steps is relatively small, the traditional method cannot effectively analyze the course change trend on the slow turning road section, and finally the linear motion detection is failed, so that the positioning accuracy of an inertial navigation system is directly affected.
Disclosure of Invention
The present invention is directed to solving the above problems of the prior art. A pedestrian navigation method based on K-step differential motion detection and track heading is provided. The technical scheme of the invention is as follows:
a pedestrian navigation method based on K-step differential motion detection and track heading comprises the following steps:
step a, adopting a strapdown inertial navigation algorithm, and calculating the gesture, speed and position parameters of pedestrian walking by utilizing the data acquired by the MEMS;
step b, detecting gait phases of pedestrian walking data acquired by the MEMS by adopting a gait phase detection algorithm;
step c, detecting the motion state of the pedestrian by using a K-step differential motion detection algorithm, judging the current motion state of the pedestrian, and extracting the track course of the straight line section;
step d, establishing a track course-position constraint model based on an extended Kalman filter;
step e, establishing a static interval constraint model based on an extended Kalman filter;
step f, compensating errors of positions, speeds and postures in the system; and repeating the steps until the positioning is stopped.
Further, in the step a, a strapdown inertial navigation algorithm is defined as follows:
in the method, in the process of the invention,for the navigation system, i.e. the n-system, three directional position vectors,/>For n is the velocity vector in three directions, < >>The attitude angle vector is a pitch angle, a roll angle and a course angle respectively; />A cosine rotation matrix for rotating the carrier coordinate system to the navigation coordinate system; />Measuring angular rate omega for gyroscopes b Is an antisymmetric matrix of (a); g n Is the gravity vector of the earth.
Furthermore, the gait phase detection algorithm in the step b adopts a maximum likelihood ratio verification method for detection, and the following formula is defined:
wherein T is the statistical detection quantity of gait detection,the acceleration value and the angular velocity value of the nth epoch are respectively represented. />Acceleration value and angular velocity value of the j-th epoch in the window; w is the detection window of generalized likelihood ratio test, sigma a Sum sigma w Variance of acceleration, angular rate, |·| represents 2-norm, ++>And->The acceleration vector and the angular rate vector which represent the output of the accelerometer and the gyroscope, g represents the gravity acceleration of an n system, and the output of the generalized likelihood ratio test is that
Wherein, gamma thr Is the threshold for gait detection.
Further, the step c uses a K-step differential motion detection algorithm to detect the motion state of the pedestrian, judges the current motion state of the pedestrian, and extracts the track course of the straight line section, which comprises the following steps:
1) Step heading: the advancing direction of the pedestrian is obtained by the positions of two adjacent steps:
in θ s (k) For the step-size heading,representing the position of step k;
2) Calculating step length:
wherein L (k) is the step length of the step k;representing the position of step k; (already described here before, it is not necessary to repeat)
3) Step length validity detection:
wherein S (k) is the result of step-size validity detection, thr L A step threshold value is set;
4) K, differential linear motion detection:
wherein N is the window size,is a set threshold value;
5) Extracting track course: under the condition of linear motion, the track course is basically kept unchanged, the track course is updated by utilizing the step length course of the current step, and is used as the course observation of the subsequent linear motion, and the track course of the path is established by adopting a window mean value mode:
in θ path (k) The obtained track course is obtained;
6) Calculating a deviation angle of the track course and the step course:
δθ s ＝θ s -θ path 。
in delta theta s Is the deviation angle of the track course and the step course.
Further, the step d establishes a track course-position constraint model based on a Kalman filter, which comprises the following steps:
the system state vector model of the extended kalman filter algorithm is:
in the method, in the process of the invention,for position error, ++>For speed error, +.>Is a mispostureDifference (S),>zero bias for gyro angular rate +.>Is acceleration zero offset;
the deviation angle between the track heading and the step heading is as follows:
δθ s ＝θ s -θ path
in θ path For the acquired track heading, θ s Is a step heading;
due to accumulated errors of inertial navigation, the position gradually deviates from the actual position; the step heading and the position are in an arctangent relationship, and a relationship between the track heading error and the position is established:
because the course error and the position are in a nonlinear relation, linearizing the above formula:
in the method, in the process of the invention,in J 11 、J 12 Expressed as:
the constraint model based on the kalman filter is therefore:
in the formula, 0 1×3 Is a three-dimensional vector of 1*3,is a track course-position observation matrix.
Further, the step e of establishing a static interval constraint model based on an extended kalman filter specifically comprises the following steps:
zero speed observation: in the walking process of the pedestrians, the motions of the feet are continuously alternated in the swinging stage and the standing stage, when the pedestrians are in the standing stage, the speed at the moment is considered to be zero, the speed in the standing stage is used as the pseudo observed quantity of the filter, and the accumulated speed error in the walking process is restrained; the zero-speed observation model is as follows:
H v ＝[0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 ]
where x is the state vector of the filter,v is the velocity error vector v To measure noise, I 3×3 An identity matrix of 3*3, 0 3×3 A zero matrix of 3*3; h v Zero velocity observation matrix, z v Is a speed error.
The pedestrians are in a standing stage at the moment, and the pedestrians are in a static state, and the positions of the pedestrians are not changed, so that the change amount of the positions is zero in a zero-speed interval; the zero position observation model is:
H p ＝[I 3×3 0 3×3 0 3×3 0 3×3 0 3×3 ]
in the method, in the process of the invention,position information obtained by a strapdown inertial navigation algorithm; />The position of the first epoch stored after entering zero speed; v p To measure noise, H p For zero position observation matrix z p Is a speed error.
Further, the step f compensates the errors of the position, the speed and the gesture in the system, and the formula is defined as follows:
in the method, in the process of the invention,is an attitude error matrix, which is obtained by converting the estimated attitude error angle, +.>For correcting the post-attitude error matrix, v n′ 、p n′ 、/>And +.>For correcting the velocity vector, position vector, adding the table zero offset and gyro zero offset, ++>And respectively estimating the obtained speed, position, adding table zero bias and gyro zero bias state errors for the extended Kalman filter.
A pedestrian navigation system employing any one of the methods of claim, comprising:
the attitude initialization module calculates an initial attitude angle of the system by using data output by the MEMS in a static state;
the K-step differential motion detection module is used for establishing a K-step differential operation model by using the step heading time sequence to detect the motion mode of the pedestrian;
an extended kalman filter module for estimating an error of the system using the extended kalman filter and compensating the error of the system using the estimated error amount.
A computer readable storage medium having stored thereon a computer program which when executed by a processor implements the pedestrian navigation method based on K-step differential motion detection and track heading of any of the above.
The invention has the advantages and beneficial effects as follows:
1. the method for detecting the K-step differential motion in the step c is a novel motion type detection method in the field of human kinematics, and can effectively step the change trend of a long-range sequence and reduce the false detection condition of motion detection; the track course can be extracted on line in real time, so that the method does not need to depend on a set dominant direction and can be suitable for more complex occasions.
2. The invention discloses a track course-position constraint model based on an extended Kalman filter in the step d, which is a novel position error correction model provided according to an inertial navigation error correction principle. The error between the step heading and the track heading is directly related to the position, so that the error caused by other parameters of the sensor can be reduced, and the positioning accuracy of the navigation of the sensor can be improved.
3. The static interval constraint model based on the extended Kalman filter in the step e can effectively constrain the speed and position errors of pedestrians in a static stage, prevent error accumulation caused by long-time integral operation, and reduce the influence of accumulated errors on navigation accuracy.
Drawings
FIG. 1 is a flow chart of a pedestrian navigation system based on K-step differential linear motion detection and trajectory heading assistance in accordance with a preferred embodiment of the present invention;
FIG. 2 is a flow chart of an extended Kalman filter;
FIG. 3 is a test path trace diagram;
FIG. 4 is a K-step differential motion detection result;
fig. 5 is a positioning trajectory result.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and specifically described below with reference to the drawings in the embodiments of the present invention. The described embodiments are only a few embodiments of the present invention.
The technical scheme for solving the technical problems is as follows:
referring to fig. 1, a pedestrian navigation algorithm based on K-step differential linear motion detection and track heading assistance includes the following steps:
step a, adopting a strapdown inertial navigation algorithm, utilizing data acquired by MEMS to calculate the gesture, speed and position parameters of pedestrian walking, and firstly initializing the gesture angle of the system, wherein the gesture angle is defined as shown in a formula XXX:
in the formula (1), θ is an initial pitch angle, γ is an initial roll angle,and g is the average value of the acceleration output by the MEMS accelerometer and is gravity acceleration.
The definition of the strapdown inertial navigation algorithm is shown in formula (2).
In the formula (2), the amino acid sequence of the compound,for the position vectors in three directions on the navigation system (n-system), +.>For n is the velocity vector in three directions, < >>The attitude angle vector is a pitch angle, a roll angle and a course angle respectively. />A cosine rotation matrix for the carrier coordinate system to the navigation coordinate system. />Measuring angular rate omega for gyroscopes b Is an anti-symmetric matrix of (a). g n Is the gravity vector of the earth.
Step b, detecting gait phases of pedestrian walking by using the data acquired by the MEMS; gait phase detection uses a maximum likelihood ratio verification method, and the definition of the maximum likelihood ratio verification is shown in a formula (3).
T is the statistical detection quantity of gait detection, W is the detection window of generalized likelihood ratio detection, sigma a Sum sigma g The variances of acceleration and angular rate, respectively, |·| represent the 2-norm,and->The acceleration vector and the angular velocity vector output by the accelerometer and the gyroscope are represented, and g represents the gravity acceleration of the n system. The output definition of the generalized likelihood ratio test is shown in equation (4).
In the formula (4), gamma thr Is the threshold for gait detection.
Step c, depending on the step b, detecting the motion state of the pedestrian by using a K-step differential motion detection algorithm after detecting that the pedestrian is in a standing stage, and extracting the track heading of the current path, wherein the motion detection comprises the following steps:
1) Step heading: the heading of the pedestrian is obtained through the positions of two adjacent steps, and is defined as shown in a formula (5).
In the formula (5), θ s (k) For the step-size heading,representing the position of step k;
2) The definition of the step size is shown in formula (6). :
in the formula (6), L (k) is the step length of k steps;representing the position of step k;
3) The definition of the step size validity detection is shown in formula (7).
In the formula (7), S (k) is the result of step effectiveness detection, thr L A step threshold value is set;
4) The definition of the K-step differential motion detection is shown in the formula (8).
In the formula (8), N is the window size,is a set threshold.
5) Extracting track course: under the condition of linear motion, the track course basically keeps unchanged, the track course is updated by utilizing the step length course of the current step, the track course can be used as the course observation of the subsequent linear motion, and the track course definition for establishing a path by adopting a window mean mode is shown in a formula (9).
In the formula (9), N is the window size, θ path (k) And (5) obtaining the track heading.
6) The deviation angle of the track heading and the step heading is calculated, and the definition is shown in a formula (10).
δθ s ＝θ s -θ path (10)
Equations (5) - (10) are steps of K-step differential motion detection and linear path track course extraction, wherein a step-length course time sequence is calculated firstly, then the sequence is analyzed by using K-step differential operation to obtain the motion state of the pedestrian, then the track course of the current road section is extracted, and finally the deviation angle between the track course and the step-length course is calculated.
And d, executing the model under the condition that the motion is detected as linear motion, gradually deviating from the actual position due to the accumulated error of inertial navigation, and directly representing the deviation of the position deviation on the deviation angle of the step heading and the track heading. And (3) establishing a track course-position constraint model based on an extended Kalman filter, wherein the definition of the model is shown in a formula (11).
In the formula (8), since the heading error and the position are nonlinear inertia, linearizing the formula (8) is performed, and a formula obtained after linearizing is shown as (10).
According to the extended Kalman filter theory, the system model definition of the filter is shown in formula (12).
In the formula (12), W (t) is white gaussian noise. The state vector definition is shown in equation (13).
In the formula (13), the amino acid sequence of the compound,for position error, ++>For speed error, +.>For posture error, ++>Zero bias for gyro angular rate +.>Is acceleration zero offset. The definition of the state transition matrix is shown in formula (14).
In the formula (14), I 3×3 Is a three-dimensional identity matrix, 0 3×3 Is a three-dimensional zero matrix, f n X is an n-series acceleration antisymmetric matrix,is an euler rotation matrix. The definition of the observation model of the filter is shown in formula (15).
Z(k)＝H k x(t)+V(t) (15)
In the formula (15), H k For the measurement matrix, V (t) is the measurement noise.
The observation equation based on the extended kalman filter is defined as shown in formula (16).
In the formula (16), the amino acid sequence of the compound,to measure noise; />A measurement matrix for track heading-position constraint is defined as shown in equation (17).
And e, when the pedestrian is in a standing stage, establishing a static interval constraint model based on an extended Kalman filter, wherein the model comprises the following steps:
zero speed observation (ZUPT): the motion of the foot is continuously alternated in the swing stage and the standing stage in the walking process of the pedestrian, when the pedestrian is in the standing stage, the speed at the moment can be considered to be zero, the speed in the standing stage is taken as the pseudo observed quantity of the filter, and the accumulated speed error in the walking process is restrained. The zero-velocity observation model is defined as shown in formula (18).
In equation (18), x is the state vector of the filter,v is the velocity error vector v To measure noise, H v A measurement matrix for zero speed constraint is defined as shown in equation (19).
H v ＝[0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 ] (19)
In the formula (19), I 3×3 An identity matrix of 3*3, 0 3×3 Is a zero matrix of 3*3.
The pedestrian is in a standing stage at the moment, and the pedestrian is in a static state, and the position of the pedestrian is not changed, so that the position change amount is zero in a zero-speed interval. The zero position observation model is defined as shown in equation (20).
In the formula (20), the amino acid sequence of the compound,to enter the position of the first epoch saved after zero speed, H p For the zero position measurement matrix, it is defined as shown in equation (21).
H p ＝[I 3×3 0 3×3 0 3×3 0 3×3 0 3×3 ] (21)
And f, after estimating the errors of the system by using an extended Kalman filter, compensating the errors of the position, the speed and the attitude in the system, wherein the error compensation is defined as shown in a formula (22).
In the method, in the process of the invention,is an attitude error matrix, which is obtained by converting the estimated attitude error angle, +.>For correcting the post-attitude error matrix, v n′ 、p n′ 、/>And +.>For correcting the velocity vector, position vector, adding the table zero offset and gyro zero offset, ++>And respectively estimating the obtained speed, position, adding table zero bias and gyro zero bias state errors for the extended Kalman filter.
According to the principle of the extended kalman filter, the implementation of the filter is divided into two parts: the time update and the measurement update are required to be performed simultaneously for each step of update in the system, and the definition of the time update is shown in a formula (23).
In the formula (23), the amino acid sequence of the compound,for one-step prediction state matrix +.>For the prediction matrix of the previous moment, Φ k,k-1 Is a discretized state transition matrix, which is defined as shown in equation (24).
Φ k,k-1 ＝I+F(t)×Δt (24)
In the formula (24), I is an identity matrix, and Δt is a sampling time of the MEMS inertial sensor.
When the time update is completed, if there is observation information, measurement update is performed, and the definition of the measurement update is shown in formula (25).
In the formula (25), K k Is the gain matrix of the filter, P k,k-1 For the covariance matrix at the previous moment, R k To measure the noise matrix, Z k To measure the innovation, P k And I is an identity matrix, which is a covariance matrix at the current moment. A flow chart of the extended kalman filter is shown in fig. 2.
Based on the established experimental platform, the IMU is fixed on the foot of the pedestrian, and the motion data of the pedestrian are acquired.
Test experiment: the test experiment is carried out on a football field of a school, the test path comprises a straight path and a curve path, when the test is started, a tester starts from a starting point, walks along a football field runway for 30 minutes, and finally stops the test after reaching the starting point. The test path is shown in fig. 3.
The final motion detection result and the positioning track result are shown in fig. 4 and 5. To verify the advantages of the motion detection method, a conventional straight line path detection method (SLP) is used with the K-step differential motion detection (LMD) herein ksd ) The movement pattern of the test path was detected, and the detection results are shown in table 1. The table shows that the detection accuracy based on the K-step differential motion detection algorithm is 96.6%, the detection accuracy of the traditional linear path detection (SLP) is 87.1%, and the detection accuracy based on the K-step differential motion detection is improved by 9.5% compared with the detection accuracy of the traditional linear path detection method. To verify the advantage of this algorithm for positioning accuracy, the positioning error of the test path is calculated using the conventional ins+ekf+zupt algorithm, based on the improved heuristic drift suppression algorithm (iHDE) and the K-step differential motion detection and trajectory heading assisted pedestrian navigation algorithm herein, respectively, and the positioning result pair is shown in table 2, and is performed by means of the mean absolute error (RMSE) indexAnd (5) evaluating. As can be seen from the table, the pedestrian navigation algorithm based on the K-step differential motion detection and the track heading assistance is reduced by about 12.61m compared with the RMSE based on the modified heuristic offset suppression algorithm (iHDE), and is reduced by about 21.81m compared with the RMSE of the traditional INS+EKF+ZUPT algorithm.
TABLE 1 Linear motion detection results
Table 2 comparison of three algorithms
The system, apparatus, module or unit set forth in the above embodiments may be implemented in particular by a computer chip or entity, or by a product having a certain function. One typical implementation is a computer. In particular, the computer may be, for example, a personal computer, a laptop computer, a cellular telephone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.
Computer readable media, including both non-transitory and non-transitory, removable and non-removable media, may implement information storage by any method or technology. The information may be computer readable instructions, data structures, modules of a program, or other data. Examples of storage media for a computer include, but are not limited to, phase change memory (PRAM), static Random Access Memory (SRAM), dynamic Random Access Memory (DRAM), other types of Random Access Memory (RAM), read Only Memory (ROM), electrically Erasable Programmable Read Only Memory (EEPROM), flash memory or other memory technology, compact disc read only memory (CD-ROM), digital Versatile Discs (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices, or any other non-transmission medium, which can be used to store information that can be accessed by a computing device. Computer-readable media, as defined herein, does not include transitory computer-readable media (transmission media), such as modulated data signals and carrier waves.
It should also be noted that the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article or apparatus that comprises the element.
The above examples should be understood as illustrative only and not limiting the scope of the invention. Various changes and modifications to the present invention may be made by one skilled in the art after reading the teachings herein, and such equivalent changes and modifications are intended to fall within the scope of the invention as defined in the appended claims.

Claims (9)

1. A pedestrian navigation method based on K-step differential motion detection and track heading is characterized by comprising the following steps:
step a, adopting a strapdown inertial navigation algorithm, and calculating the gesture, speed and position parameters of pedestrian walking by utilizing the data acquired by the MEMS;
step b, detecting gait phases of pedestrian walking data acquired by the MEMS by adopting a gait phase detection algorithm;
step c, detecting the motion state of the pedestrian by using a K-step differential motion detection algorithm, judging the current motion state of the pedestrian, and extracting the track course of the straight line section;
step d, establishing a track course-position constraint model based on an extended Kalman filter;
step e, establishing a static interval constraint model based on an extended Kalman filter;
step f, compensating errors of positions, speeds and postures in the system; and repeating the steps until the positioning is stopped.
2. The pedestrian navigation method based on K-step differential motion detection and track heading according to claim 1, wherein in the step a, a strapdown inertial navigation algorithm is defined as follows:
in the method, in the process of the invention,for the navigation system, i.e. the n-system, three directional position vectors,/>For n is the velocity vector in three directions, < >>The attitude angle vector is a pitch angle, a roll angle and a course angle respectively; />A cosine rotation matrix for rotating the carrier coordinate system to the navigation coordinate system; />Measuring angular rate omega for gyroscopes b Is an antisymmetric matrix of (a); g n Is the gravity vector of the earth.
3. The pedestrian navigation method based on the K-step differential motion detection and the track heading according to claim 1, wherein the gait phase detection algorithm in the step b adopts a maximum likelihood ratio verification method for detection, and the following formula is defined:
wherein T is the statistical detection quantity of gait detection,respectively representing the acceleration value and the angular velocity value of the nth epoch,acceleration value and angular velocity value of the j-th epoch in the window; w is the detection window of generalized likelihood ratio test, sigma a Sum sigma w Variance of acceleration, angular rate, |·| represents 2-norm, ++>And->The acceleration vector and the angular rate vector which represent the output of the accelerometer and the gyroscope, g represents the gravity acceleration of an n system, and the output of the generalized likelihood ratio test is that
Wherein, gamma thr Is the threshold for gait detection.
4. The pedestrian navigation method based on the K-step differential motion detection and the track heading according to claim 1, wherein the step c uses the K-step differential motion detection algorithm to detect the motion state of the pedestrian, judges the current motion state of the pedestrian, and extracts the track heading of the straight line section, and specifically comprises:
1) Step heading: the advancing direction of the pedestrian is obtained by the positions of two adjacent steps:
in θ s (k) For the step-size heading,representing the position of step k;
2) Calculating step length:
wherein L (k) is the step length of the step k;
3) Step length validity detection:
wherein S (k) is the result of step-size validity detection, thr L A step threshold value is set;
4) K, differential linear motion detection:
wherein N is the window size,is a set threshold value;
5) Extracting track course: under the condition of linear motion, the track course is basically kept unchanged, the track course is updated by utilizing the step length course of the current step, and is used as the course observation of the subsequent linear motion, and the track course of the path is established by adopting a window mean value mode:
in θ path (k) The obtained track course is obtained;
6) Calculating a deviation angle of the track course and the step course:
δθ s ＝θ s -θ path 。
in delta theta s Is the deviation angle of the track course and the step course.
5. The method for pedestrian navigation based on K-step differential motion detection and trajectory heading as claimed in claim 4, wherein said step d of establishing a kalman filter-based trajectory heading-position constraint model includes:
the system state vector model of the extended kalman filter algorithm is:
in the method, in the process of the invention,for position error, ++>For speed error, +.>For posture error, ++>Zero bias for gyro angular rateIs acceleration zero offset;
the deviation angle between the track heading and the step heading is as follows:
δθ s ＝θ s -θ path
in θ path For the acquired track heading, θ s Is a step heading;
due to accumulated errors of inertial navigation, the position gradually deviates from the actual position; the step heading and the position are in an arctangent relationship, and a relationship between the track heading error and the position is established:
because the course error and the position are in a nonlinear relation, linearizing the above formula:
in the method, in the process of the invention,intermediate variable J 11 、J 12 Expressed as:
the constraint model based on the kalman filter is therefore:
in the formula, 0 1×3 Is a three-dimensional vector of 1*3.
6. The pedestrian navigation method based on the K-step differential motion detection and the track heading according to claim 1, wherein the step e establishes a static interval constraint model based on an extended kalman filter, and specifically comprises the following steps:
zero speed observation: in the walking process of the pedestrians, the motions of the feet are continuously alternated in the swinging stage and the standing stage, when the pedestrians are in the standing stage, the speed at the moment is considered to be zero, the speed in the standing stage is used as the pseudo observed quantity of the filter, and the accumulated speed error in the walking process is restrained; the zero-speed observation model is as follows:
H v ＝[0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 ]
where x is the state vector of the filter,v is the velocity error vector v To measure noise, I 3×3 An identity matrix of 3*3, 0 3×3 A zero matrix of 3*3; h v Zero velocity observation matrix, z v Is a speed error;
the pedestrians are in a standing stage at the moment, and the pedestrians are in a static state, and the positions of the pedestrians are not changed, so that the change amount of the positions is zero in a zero-speed interval; the zero position observation model is:
H p ＝[I 3×3 0 3×3 0 3×3 0 3×3 0 3×3 ]
in the method, in the process of the invention,position information obtained by a strapdown inertial navigation algorithm; />To enter the first one of the zero-speed post-saveThe location of the epoch; v p To measure noise, H p For zero position observation matrix z p Is a speed error.
7. The pedestrian navigation method based on K-step differential motion detection and trajectory heading according to claim 1, wherein the step f compensates for errors in position, speed and attitude in the system, and the formula is defined as follows:
in the method, in the process of the invention,is an attitude error matrix, which is obtained by converting the estimated attitude error angle, +.>For correcting the post-attitude error matrix, v n′ 、p n′ 、/>And +.>For correcting the velocity vector, position vector, adding the table zero offset and gyro zero offset, ++>And respectively estimating the obtained speed, position, adding table zero bias and gyro zero bias state errors for the extended Kalman filter.
8. A pedestrian navigation system employing the method of any one of claims 1-7, comprising:
the attitude initialization module calculates an initial attitude angle of the system by using data output by the MEMS in a static state;
the K-step differential motion detection module is used for establishing a K-step differential operation model by using the step heading time sequence to detect the motion mode of the pedestrian;
an extended kalman filter module for estimating an error of the system using the extended kalman filter and compensating the error of the system using the estimated error amount.
9. A computer readable storage medium having stored thereon a computer program, which when executed by a processor implements the pedestrian navigation method based on K-step differential motion detection and track heading of any one of claims 1 to 7.
CN202311138383.4A 2023-09-05 2023-09-05 Pedestrian navigation method and system based on K-step differential motion detection and track heading Pending CN117268377A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311138383.4A CN117268377A (en) 2023-09-05 2023-09-05 Pedestrian navigation method and system based on K-step differential motion detection and track heading

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311138383.4A CN117268377A (en) 2023-09-05 2023-09-05 Pedestrian navigation method and system based on K-step differential motion detection and track heading

Publications (1)

Publication Number Publication Date
CN117268377A true CN117268377A (en) 2023-12-22

Family

ID=89216871

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311138383.4A Pending CN117268377A (en) 2023-09-05 2023-09-05 Pedestrian navigation method and system based on K-step differential motion detection and track heading

Country Status (1)

Country Link
CN (1) CN117268377A (en)

Similar Documents

Publication Publication Date Title
JP6783751B2 (en) Methods and equipment to use portable navigation with improved quality of map information assistance
Jiménez et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU
Krach et al. Cascaded estimation architecture for integration of foot-mounted inertial sensors
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
Wu et al. Low-cost antenna attitude estimation by fusing inertial sensing and two-antenna GPS for vehicle-mounted satcom-on-the-move
CN112639502A (en) Robot pose estimation
Eling et al. Development of an instantaneous GNSS/MEMS attitude determination system
Stančić et al. The integration of strap-down INS and GPS based on adaptive error damping
CN111721288B (en) Zero offset correction method and device for MEMS device and storage medium
Ju et al. A pedestrian dead-reckoning system that considers the heel-strike and toe-off phases when using a foot-mounted IMU
Anjum et al. Sensor data fusion using unscented kalman filter for accurate localization of mobile robots
US11162791B2 (en) Method and system for point of sale ordering
Benzerrouk et al. Robust IMU/UWB integration for indoor pedestrian navigation
US8812234B2 (en) Device to aid navigation, notably inside buildings
Ali et al. Performance comparison among some nonlinear filters for a low cost SINS/GPS integrated solution
US20160179190A1 (en) Method and apparatus for estimating position of pedestrian walking on locomotion interface device
Wu et al. Low-cost attitude estimation with MIMU and two-antenna GPS for Satcom-on-the-move
CN110672095A (en) Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation
Cai et al. An online smoothing method based on reverse navigation for ZUPT-aided INSs
CN112362057A (en) Inertial pedestrian navigation algorithm based on zero-speed correction and attitude self-observation
Anbu et al. Integration of inertial navigation system with global positioning system using extended kalman filter
Cucci et al. An analysis of a gyro-free inertial system for INS/GNSS navigation
Ilyas et al. Drift reduction in IMU-only pedestrian navigation system in unstructured environment
CN117268377A (en) Pedestrian navigation method and system based on K-step differential motion detection and track heading
CN113959433B (en) Combined navigation method and device

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