CN114608571A - Pedestrian inertial navigation method suitable for motion platform scene - Google Patents

Pedestrian inertial navigation method suitable for motion platform scene Download PDF

Info

Publication number
CN114608571A
CN114608571A CN202210180015.5A CN202210180015A CN114608571A CN 114608571 A CN114608571 A CN 114608571A CN 202210180015 A CN202210180015 A CN 202210180015A CN 114608571 A CN114608571 A CN 114608571A
Authority
CN
China
Prior art keywords
speed
representing
zero
correction
velocity
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
CN202210180015.5A
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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202210180015.5A priority Critical patent/CN114608571A/en
Publication of CN114608571A publication Critical patent/CN114608571A/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
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B5/00—Measuring for diagnostic purposes; Identification of persons
    • A61B5/103—Detecting, measuring or recording devices for testing the shape, pattern, colour, size or movement of the body or parts thereof, for diagnostic purposes
    • A61B5/11—Measuring movement of the entire body or parts thereof, e.g. head or hand tremor, mobility of a limb
    • A61B5/112—Gait analysis
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems

Abstract

The invention discloses a pedestrian inertial navigation method suitable for a motion platform scene, which comprises the following steps: (1) acquiring pedestrian foot binding type IMU data, and estimating a zero-speed detection threshold; (2) preprocessing data, and resolving speed, attitude and position in real time; (3) establishing a gait detection model, and carrying out zero-speed detection by utilizing an estimated zero-speed detection threshold value; (4) when the device is in a zero-speed state, a Kalman filter is used for carrying out zero-speed correction; otherwise, no correction is carried out; (5) comparing the related speed obtained by resolving in the step (2) with a set threshold value to judge whether a motion platform and the type of the motion platform exist at the current moment; when the existence of a certain motion platform is detected, sub-states of the motion of the platform are classified, different speed constraints are applied, and the speed, the posture and the position of a pedestrian are corrected by using a filtering method; and if the motion platform is not detected, repeating the steps (2) - (5). The invention solves the problem of inaccurate pedestrian inertial navigation positioning in a motion platform scene.

Description

Pedestrian inertial navigation method suitable for motion platform scene
Technical Field
The invention belongs to the technical field of pedestrian inertial navigation systems, and relates to a pedestrian inertial navigation method suitable for a motion platform scene.
Background
The indoor pedestrian positioning service is a core support technology in the fields of internet of things ((IoT)), big data, Artificial Intelligence (AI) and the like, and with the rapid development of the technologies, the importance of indoor positioning is increasingly prominent. Based on rapid development and popularization of MEMS technology, a pedestrian navigation system based on MEMS inertial sensors is realized, a common pedestrian inertial navigation system is characterized in that the inertial sensors are fixedly connected to feet of pedestrians, and the system can periodically realize the correction of navigation errors by combining the special gait characteristics of foot motion and utilizing a Zero Velocity update (ZUPT) method on the basis of the traditional SINS algorithm. Therefore, the zero-speed correction plays a crucial role in the inertial navigation system, and is an effective means for restraining the divergence of inertial navigation errors.
The existing pedestrian inertial navigation method only carries out relevant research on static ground scenes, however, when a pedestrian takes an external moving platform such as a vertical elevator or an escalator in a high-rise building such as an office building and a shopping mall, a zero-speed detection algorithm is often mistakenly detected to be in a zero-speed state, so that wrong speed observation is formed, and speed and position errors of the system are rapidly dispersed. Therefore, in the scene of an external motion platform, speed error constraint or wrong speed error constraint is not carried out, and the requirements of high-precision and reliable pedestrian positioning navigation cannot be met.
Disclosure of Invention
The purpose of the invention is as follows: aiming at the defects, the invention provides the pedestrian inertial navigation method suitable for the motion platform scene, solves the problem of inaccurate pedestrian inertial navigation positioning in the motion platform scene, reduces the accumulated error of pedestrian navigation positioning, and improves the navigation positioning precision.
The technical scheme is as follows: in order to solve the problems, the invention provides a pedestrian inertial navigation method suitable for a motion platform scene, which comprises the following steps:
(1) acquiring pedestrian foot binding IMU data, and estimating a zero-speed detection threshold value through initial static data;
(2) preprocessing the acquired foot-bound IMU data and resolving the speed, the attitude and the position in real time by adopting a strapdown inertial navigation method;
(3) establishing a gait detection model of four nodes of the leg and the foot, and performing zero-speed detection by using an estimated zero-speed detection threshold;
(4) when the zero-speed detection result is in a zero-speed state, performing zero-speed correction by using a Kalman filter to obtain the corrected speed, posture and position, and performing error correction on the IMU device; otherwise, no correction is carried out;
(5) comparing the related speed obtained by resolving in the step (2) with a set threshold value to judge whether a motion platform and the type of the motion platform exist at the current moment; when the existence of a certain motion platform is detected, sub-states of the motion of the certain motion platform are synchronously classified, different speed constraints are applied according to the motion characteristics of different sub-states, and the speed, the posture and the position of a pedestrian are corrected by using a filtering method; and if the motion platform is not detected, repeating the steps (2) - (5).
Further, the motion platform detection and classification in the step (5) specifically comprises:
if the following conditions are met:
Figure BDA0003520137820000021
then the pedestrian is positioned at the escalator at the moment k; wherein, VkRepresents a velocity vector in the geographic region,
Figure BDA0003520137820000022
representing the velocity vector, T, in the direction of the skyescal,normIndicating the speed threshold, T, of the escalatorescal,URepresenting a speed threshold of the escalator in the direction of the sky;
if the following conditions are met:
Figure BDA0003520137820000023
then the pedestrian is positioned in the vertical elevator at the moment k and the elevator starts to run; wherein, VkRepresents a velocity vector in the geographic region,
Figure BDA0003520137820000024
representing the velocity vector, T, in the direction of the skyele,normIndicating the speed threshold of the vertical elevator, Tele,URepresenting the vertical elevator speed threshold in the direction of the day.
Furthermore, when the escalator is located at the current moment, the whole-stroke motion state of the escalator is divided into five sub-states S11-S15, and the five sub-states are switched in a fixed sequence; wherein, substate S11 shows that the pedestrian is in the staircase entry, adopts the speed restraint method to be: firstly, performing lateral zero-speed correction and then performing vertical zero-speed correction; the sub-state S12 represents a pedestrian positioned at a corner near the entrance of the escalator, using the speed constraint method: firstly, performing lateral zero-speed correction and then performing constant speed modulus correction; the substate S13 shows the pedestrian is on the escalator, using the speed constraint method: firstly, performing lateral zero-speed correction and then performing forward and vertical constant-speed correction; sub-state S14 represents a corner position near the exit of the escalator, using the speed constraint method: firstly, performing lateral zero-speed correction and then performing constant speed modulus correction; the sub-state S11 represents a pedestrian at the exit of the escalator, using the speed constraint method: firstly, performing lateral zero-speed correction and then performing vertical zero-speed correction;
when the current time is in the vertical elevator, the whole-course motion state of the vertical elevator is divided into three sub-states S21-S23, and the five sub-states are switched in a fixed sequence; substate S21 represents a vertical elevator acceleration traveling upwards, and the method of using the speed constraint is: carrying out horizontal zero-speed correction; substate S22 represents a constant speed up run of the vertical elevator, the method of using speed constraints being: firstly, carrying out horizontal zero-speed correction and then carrying out vertical constant-speed correction; substate S23 represents a vertical elevator traveling at reduced speed and upward, and the method of using speed constraints is: and carrying out horizontal zero-speed correction.
Further, the lateral zero velocity correction method, the horizontal zero velocity correction method and the vertical zero velocity correction method are all low-dimensional representations of the zero velocity correction method, and the specific formulas are as follows:
Zzupt,lateral＝δVlateral＝VE-0＝HlateralX+vlateral。
Zzupt,horiz＝δVhoriz＝[VE VN]T-[0 0]T＝HhorizX+vhoriz
Zzupt,vert＝δVvert＝VU-0＝HvertX+vvert
wherein Z iszupt,lateralObservation quantity Z representing lateral zero-speed correctionzupt_horizObservation quantity Z representing horizontal zero-speed correctionzupt,vertRepresenting the observed quantity of the vertical zero speed correction; delta VlateralIMU velocity error quantity, delta V, representing lateral zero velocity correctionhorizRepresentIMU speed error amount, delta V, of horizontal zero speed correctionvertAn IMU velocity error amount representing a vertical velocity correction; vEVelocity vector V representing east directionNVelocity vector, V, representing northUA velocity vector representing a direction of the day; hlateral＝[01×3 1 01×14]，Hhoriz＝[02×3 I2×2 02×13]，Hvert＝[01×5 1 01×12]；vlateralObservation noise matrix, v, representing lateral zero-velocity correctionhorizObservation noise matrix, v, representing lateral zero-velocity correctionvertAn observed noise matrix representing a vertical zero-velocity correction;
the formula of the constant speed modulus correction method is as follows:
Figure BDA0003520137820000031
Figure BDA0003520137820000032
Zcons_norm＝δVk,norm＝Vk,norm-Vob,norm＝f(Vk)-Vob,norm+nn
wherein, T11And T12Indicating entry into substate S11And S12The time of (d); vob,normRepresenting a kinematic sub-state S11Mean value, V, of velocity vector modulus obtained by solution in the processk,normRepresenting the modulus value of the velocity vector at time k,
Figure BDA0003520137820000033
an east velocity vector representing the k time,
Figure BDA0003520137820000034
A velocity vector representing the north direction at time k,
Figure BDA0003520137820000035
Indicating the time of kA velocity vector in the direction of the sky; f () represents a modulo function, Zcons_normIndicating the observed quantity, deltaV, of constant correction of the velocity mode valuek,normRepresenting the velocity module error, n, of the fully bound IMU at time knTo observe the noise matrix;
the forward direction and vertical direction velocity vector constant correction method observation equation formula is as follows:
Figure BDA0003520137820000041
wherein Z iscons_vectRepresenting the velocity error observed quantity of velocity vector constant correction in the forward direction and the vertical direction; delta Vk,vectRepresents the difference between the east and north velocity vectors at time k and time k-1, nvTo observe the noise matrix; hcons_vect＝[02×3 I2×202×13]。
Further, the classification algorithm formula of the sub-states is as follows:
Figure BDA0003520137820000042
wherein, Countk,fpDenotes the forward search tag, N denotes the sliding window length, sgn () denotes the sign function, AURepresenting the output of the acceleration specific force in the sky direction, wherein g is a local gravity acceleration value;
when the moving platform is an escalator, the moving platform is a ladder,
when Count k,fp1 denotes that at time k, the substate switches to S11；
When 0.9N is less than or equal to Countk,fpN ≦ N, meaning that at time k, the substate is from S11Switching to S12；
when-0.2N is less than or equal to Countk,fp≦ 0.2N, meaning that at time k, the substate is from S12Switching to S13；
when-N is less than or equal to Countk,fp≦ 0.9N, meaning that at time k, the substate is from S13Switching to S14；
when-0.2N is less than or equal to Countk,fpLess than or equal to 0.2N, which means that at the time k, the substate is represented by S14Switching to S15；
When the moving platform is a vertical elevator,
when 0.9N is less than or equal to Countk,fpN is less than or equal to the sum of the values of the sub-states of the pedestrian and the S, and the pedestrian is in the vertical elevator and starts to move at the moment k21；
when-0.2N is less than or equal to Countk,fp≦ 0.2N, meaning that at time k, the substate is from S21Switching to S22；
when-N is less than or equal to Countk,fp≦ 0.9N, meaning that at time k, the substate is from S22Switching to S23。
Further, the zero-velocity detection threshold estimated in step (1) is specifically: the center of the foot-bound inertial sensor is taken as a coordinate origin O, the right direction is taken as an X axis, a Y axis is perpendicular to the X axis and points to the front direction, and a Z axis points to the upper direction; acquiring pedestrian foot binding IMU data, and acquiring a zero-speed detection threshold value according to the data characteristics of acceleration and angular speed in an initial static interval; the detection threshold comprises a minimum threshold T of an acceleration modulus valueA,minMaximum threshold value T of acceleration modulusA,maxAcceleration standard deviation threshold
Figure BDA0003520137820000043
Threshold value T of angular velocity modulusWAngular velocity standard deviation threshold
Figure BDA0003520137820000044
The calculation formula is as follows:
Figure BDA0003520137820000051
wherein A isstaicIs the mean, σ, of the acceleration mode in the static intervalA_staticIs the standard deviation, W, of the acceleration mode in the static intervalstaicIs the mean, σ, of the angular velocity mode in the static intervalW_staticThe standard deviation of the angular velocity mode in the static interval; c. CA,minExpressed as the minimum constant coefficient of the acceleration modulus、cA,maxExpressed as the maximum constant coefficient of the acceleration modulus,
Figure BDA0003520137820000052
expressed as the standard deviation of acceleration,
Figure BDA0003520137820000053
Modulus, c, expressed as angular velocityWExpressed as a constant coefficient of standard deviation.
Further, the step (2) specifically comprises the following steps:
(2.1) calculating and deducting accelerometer zero-bias epsilon from IMU static databAnd zero bias of gyroscope
Figure BDA0003520137820000054
(2.2) calculating an initial attitude angle by using the specific force output of the accelerometer under the static state, wherein the initial attitude angle mainly comprises a pitch angle theta and a roll angle gamma:
Figure BDA0003520137820000055
Figure BDA0003520137820000056
wherein, gxSpecific force output of an X axis of the static lower foot binding type IMU; gySpecific force output of the Y axis of the static lower foot binding type IMU; gzSpecific force output of the Z axis of the static lower foot binding type IMU;
(2.3) resolving the attitude, speed and position of the pedestrian in real time
The posture updating algorithm formula of the foot-bound IMU is as follows:
Figure BDA0003520137820000057
wherein Q represents a quaternion matrix, tk-1Denotes the time k-1, tkIndicating the time k, i.e. the time k-1The next sampling instant of (a); ψ represents a heading angle, θ represents a pitch angle, γ represents a roll angle,
Figure BDA0003520137820000058
representing a gesture;
the velocity update algorithm formula of the foot-bound IMU is as follows:
Figure BDA0003520137820000059
wherein, VkIs a motion velocity vector V in the geographic system at the moment kk-1The motion velocity vector is the motion velocity vector under the geographic system at the moment k-1;
Figure BDA00035201378200000510
a coordinate transformation matrix representing the machine system to the navigation system,
Figure BDA00035201378200000511
represents specific force, gnRepresents the local gravitational acceleration, Δ t represents the sampling time interval;
the position updating algorithm formula of the foot-bound IMU is as follows:
Figure BDA0003520137820000061
wherein L represents longitude, λ represents latitude, and h represents altitude; rmRepresents the radius of curvature, R, in the meridian plane of the earthnAnd (3) representing the inner curvature radius of the earth prime plane.
Further, the gait detection models of the four nodes of the leg and the foot in the step (3) are as follows:
Figure BDA0003520137820000062
Figure BDA0003520137820000063
Figure BDA0003520137820000064
Figure BDA0003520137820000065
ZVD(k)＝S1(k)&S2(k)&S3(k)&S4(k)
wherein the content of the first and second substances,
Figure BDA0003520137820000066
represents the mean value, σ, of the accelerations at the k-th to k + N momentsA,k:k+nRepresents the standard deviation of the acceleration at the k-th to k + N-th time points,
Figure BDA0003520137820000067
Represents the mean value, σ, of the angular velocities at the k-th to k + N-th momentsW,k:k+NRepresents the standard deviation of the angular velocity at the k-th to k + N-th moments; t isA,minIs the minimum threshold value, T, of the accelerationA,maxIs a maximum threshold value of the acceleration,
Figure BDA0003520137820000068
Is a threshold value, T, of the standard deviation of the accelerationWIs a threshold value of angular velocity,
Figure BDA0003520137820000069
A threshold value of the standard deviation of angular velocity; when S is1(k)、S2(k)、S3(k)、S4(k) When 1 is simultaneously set, and 1 is simultaneously set, zvd (k) becomes 1, indicating that the zero speed state is currently set.
Further, the step (4) specifically comprises the following steps:
(4.1) when the detected foot is in a zero-speed state, performing error estimation by using Kalman filtering calculation to construct an 18-dimensional Kalman filter state quantity X:
Figure BDA00035201378200000610
wherein the content of the first and second substances,
Figure BDA0003520137820000071
respectively representing platform error angles of east direction, north direction and sky direction; δ V ═ δ VEδVN δVU]TSpeed errors in the east direction, the north direction and the sky direction are respectively; δ P ═ δ L δ λ δ h]TRespectively representing errors of longitude, latitude and altitude;
Figure BDA0003520137820000072
respectively representing zero offset errors of an X axis, a Y axis and a Z axis of the gyroscope;
Figure BDA0003520137820000073
respectively representing first-order Markov drift errors of an X axis, a Y axis and a Z axis of the gyroscope;
Figure BDA0003520137820000074
respectively representing first-order Markov drift errors of an X axis, a Y axis and a Z axis of the accelerometer;
(4.2) establishing a state equation:
Figure BDA0003520137820000075
wherein A represents a state transition matrix, G represents a system noise transition matrix, and W represents system noise;
(4.3) establishing a measurement equation:
Zzupt＝δV＝VINS-VZUPT＝HX+v
wherein Z iszuptRepresenting the observed quantity, i.e. IMU speed error quantity delta V, VINSIMU velocity vector, V, resolved for strapdown inertial navigationZUPTTheoretical value of [ 000]TH is an observation matrix, v is an observation noise matrix and represents the uncertainty of speed observation;
and (4.4) correcting the navigation error and correcting the IMU device error:
constructing a Kalman filter according to the steps (4.1), (4.2) and (4.3), and solving to obtain the optimal estimation state quantity
Figure BDA0003520137820000076
Deducing the former nine-dimensional state quantity from the strapdown inertial navigation resolving result in the step (2)
Figure BDA0003520137820000077
Obtaining the corrected attitude, speed and position, and deducting the nine-dimensional state quantity from the IMU data
Figure BDA0003520137820000078
And correcting the zero offset error and the first-order Markov drift error of the IMU.
Further, the step (5) of correcting the speed, the posture and the position of the pedestrian by using a filtering method specifically comprises the following steps of;
Figure BDA0003520137820000079
Figure BDA00035201378200000710
Figure BDA00035201378200000711
Figure BDA00035201378200000712
wherein A represents a state transition matrix, Q represents a state transition covariance matrix,
Figure BDA00035201378200000713
representing the prior estimated covariance, P, of time kk-1Representing the posteriori estimated covariance, K, at time K-1kRepresenting the Kalman filter gain, J representing the Jacobian matrix, and R representing the measurement noiseAcoustic covariance, Z represents the observed quantity,
Figure BDA00035201378200000714
representing the state prior estimate, X, at time k derived from the system equationkThe optimal estimate obtained by the Kalman filter solution,
Figure BDA00035201378200000715
representing the posterior estimation covariance at the time k, and I representing an identity matrix;
solving the optimal estimation error state quantity XkThereafter, the position, velocity, attitude and IMU device errors are corrected again.
Has the advantages that: compared with the prior art, the invention has the following remarkable advantages: by analyzing the motion characteristics of different motion platforms in different states, different velocity constraints are applied in a targeted manner, and the velocity, the posture and the position of a pedestrian are corrected by combining a filtering method, so that the problem of reliable navigation of the pedestrian inertia in a motion platform scene is effectively solved, and the error divergence of an inertial navigation system is effectively inhibited.
Drawings
FIG. 1 is a flow chart of the present invention;
fig. 2 is a state classification diagram of the escalator of the present invention;
FIG. 3 is a schematic view of the present invention showing the state classification of the vertical electric ladder;
FIG. 4 is a graph comparing the positioning result of the escalator of the present embodiment with that of the conventional method;
fig. 5 is a graph comparing the results of the vertical elevator positioning according to the present embodiment with those of the conventional method.
Detailed Description
The technical scheme of the invention is further explained by combining the attached drawings.
As shown in fig. 1, a pedestrian inertial navigation method suitable for a moving platform scene includes the following steps:
the method comprises the following steps: and acquiring pedestrian foot-bound IMU data, and acquiring a zero-speed detection threshold value by using 1000 static data output by the foot-bound IMU.
Specifically, the center of the foot-bound inertial sensor is taken as a coordinate origin O, the right direction is taken as an X axis, a Y axis is perpendicular to the X axis and points forward, a Z axis points upward, and the three axes meet the right-hand spiral rule. 1000 static data output by the foot-bound inertial sensor are collected, and the acceleration and angular velocity threshold value of zero-speed detection is obtained according to the data characteristics of acceleration and angular velocity in the initial static interval.
The detection threshold comprises a minimum threshold T of an acceleration modulus valueA,minMaximum threshold value T of acceleration modulusA,maxAcceleration standard deviation threshold
Figure BDA0003520137820000081
Threshold value T of angular velocity modulusWAngular velocity standard deviation threshold
Figure BDA0003520137820000082
The calculation formula is as follows:
Figure BDA0003520137820000083
wherein A isstaicIs the mean, σ, of the acceleration mode in the static intervalA_staticIs the standard deviation, W, of the acceleration mode in the static intervalstaicIs the mean, σ, of the angular velocity mode in the static intervalW_staticThe standard deviation of the angular velocity mode in the static interval; c. CA,minExpressed as the minimum constant coefficient of the acceleration modulus, cA,maxExpressed as the maximum constant coefficient of the acceleration modulus,
Figure BDA0003520137820000091
expressed as the standard deviation of acceleration,
Figure BDA0003520137820000092
Modulus, c, expressed as angular velocityWExpressed as a constant coefficient of standard deviation.
Step two: and at the initial static moment, performing initial attitude solution and zero offset calibration of the device, and then performing real-time solution by using a strapdown inertial navigation method.
(1) Calculating and deducting accelerometer zero bias epsilon from IMU static databAnd zero bias of gyroscope
Figure BDA0003520137820000093
(2) Calculating an initial attitude angle by using specific force output of the accelerometer under a static state, wherein the initial attitude angle mainly comprises a pitch angle theta and a roll angle gamma:
Figure BDA0003520137820000094
Figure BDA0003520137820000095
wherein, gxSpecific force output of an X axis of the static lower foot binding type IMU; gySpecific force output of the Y axis of the static lower foot binding type IMU; gzSpecific force output of the Z axis of the static lower foot binding type IMU;
(3) real-time resolving the attitude, speed and position of the pedestrian
The posture updating algorithm formula of the foot-bound IMU is as follows:
Figure BDA0003520137820000096
Figure BDA0003520137820000097
wherein Q represents a quaternion matrix, tk-1Denotes the time k-1, tkRepresenting the time k, the next sample time of time k-1, delta theta represents the angular increment of the triaxial of the gyroscope over the sample time, delta theta represents the antisymmetric matrix formed by delta theta, psi represents the heading angle, theta represents the pitch angle, gamma represents the roll angle,
Figure BDA0003520137820000098
representing a gesture;
the velocity update algorithm formula of the foot-bound IMU is as follows:
Figure BDA0003520137820000099
wherein, VkIs a motion velocity vector V in the geographic system at the moment kk-1The motion velocity vector is the motion velocity vector under the geographic system at the moment k-1;
Figure BDA00035201378200000910
a coordinate transformation matrix representing the machine system to the navigation system,
Figure BDA00035201378200000911
represents specific force, gnRepresents the local gravitational acceleration, Δ t represents the sampling time interval;
the position updating algorithm formula of the foot-bound IMU is as follows:
Figure BDA0003520137820000101
wherein L represents longitude, λ represents latitude, and h represents altitude; rmRepresents the radius of curvature, R, in the meridian plane of the earthnRepresenting the inner curvature radius of the earth prime plane; vEVelocity vector V representing east directionNVelocity vector, V, representing northURepresenting a velocity vector in the direction of the day.
Step three: and establishing a gait detection model, and carrying out zero-velocity detection by taking the acceleration module value, the foot acceleration standard deviation, the angular velocity module value and the angular velocity standard deviation as detection conditions and combining with an estimated zero-velocity detection threshold value. The motion state of the foot is divided into a swing phase and a support phase, when the zero-speed state is detected, the foot is in the support phase, otherwise, the foot is in the swing phase.
Specifically, the gait detection model of four nodes of the leg and the foot is as follows:
Figure BDA0003520137820000102
Figure BDA0003520137820000103
Figure BDA0003520137820000104
Figure BDA0003520137820000105
ZVD(k)＝S1(k)&S2(k)&S3(k)&S4(k)
wherein the content of the first and second substances,
Figure BDA0003520137820000106
represents the mean value, σ, of the accelerations at the k-th to k + N momentsA,k:k+nRepresents the standard deviation of the acceleration at the k-th to k + N-th time points,
Figure BDA0003520137820000107
Represents the mean value, σ, of the angular velocities at the k-th to k + N-th momentsW,k:k+NRepresents the standard deviation of the angular velocity at the k-th to k + N-th moments; t isA,minIs the minimum threshold value, T, of the accelerationA,maxIs a maximum threshold value of the acceleration,
Figure BDA0003520137820000108
Is a threshold value, T, of the standard deviation of the accelerationWIs a threshold value of angular velocity,
Figure BDA0003520137820000109
A threshold value of the standard deviation of angular velocity; when the label S1(k)、S2(k)、S3(k)、S4(k) At the same time, when 1, zvd (k) is 1, which indicates that the zero speed state is currently set.
Step four: in the calculation process of the strapdown inertial navigation system, when the foot is detected to be in a zero-speed state, error estimation is carried out by utilizing Kalman filtering calculation to obtain the corrected position and speed, otherwise, correction is not carried out;
(4.1) when the detected foot is in a zero-speed state, performing error estimation by using Kalman filtering calculation to construct an 18-dimensional Kalman filter state quantity X:
Figure BDA0003520137820000111
wherein the content of the first and second substances,
Figure BDA0003520137820000112
respectively representing platform error angles of east, north and sky directions; δ V ═ δ VEδVN δVU]TSpeed errors in the east direction, the north direction and the sky direction are respectively; δ P ═ δ L δ λ δ h]TRespectively representing errors of longitude, latitude and height;
Figure BDA0003520137820000113
respectively representing zero offset errors of an X axis, a Y axis and a Z axis of the gyroscope;
Figure BDA0003520137820000114
respectively representing first-order Markov drift errors of an X axis, a Y axis and a Z axis of the gyroscope;
Figure BDA0003520137820000115
respectively representing first-order Markov drift errors of an X axis, a Y axis and a Z axis of the accelerometer;
(4.2) establishing a state equation:
Figure BDA0003520137820000116
wherein A represents a state transition matrix, G represents a system noise transition matrix, and W represents system noise;
Figure BDA0003520137820000117
is one of the state quantity XA first derivative; w ═ Wgx wgy wgz]T，wgxWhite noise of gyro X-axis, wgyWhite noise, w, for the gyro Y axisgzWhite noise of the Z axis of the gyroscope;
(4.3) establishing a measurement equation:
Zzupt＝δV＝VINS-VZUPT＝HX+v
wherein Z iszuptRepresenting the observed quantity, i.e. IMU speed error quantity delta V, VINSIMU velocity vector, V, resolved for strapdown inertial navigationZUPTTheoretical value of [ 000]TH is an observation matrix, v is an observation noise matrix, and represents the uncertainty of velocity observation;
and (4.4) correcting the navigation error and correcting the IMU device error:
constructing a Kalman filter according to the steps (4.1), (4.2) and (4.3), and solving to obtain the optimal estimation state quantity
Figure BDA0003520137820000118
Namely, estimation errors of attitude, velocity, position; deducing the first nine-dimensional state quantity from the second strapdown inertial navigation resolving result
Figure BDA0003520137820000119
Obtaining the corrected attitude, speed and position, and deducting the nine-dimensional state quantity from the IMU data
Figure BDA00035201378200001110
And correcting the zero offset error and the first-order Markov drift error of the IMU.
Step five: comparing the related speed obtained by resolving in the second step with a set threshold value to judge whether a motion platform and the type of the motion platform exist at the current moment; when the existence of a certain motion platform is detected, sub-states of the motion of the certain motion platform are synchronously classified, different speed constraints are applied according to the motion characteristics of different sub-states, and the speed, the posture and the position of a pedestrian are corrected by using a filtering method; and if the motion platform is not detected, repeating the step two to the step. The method comprises the following specific steps:
(1) detecting and classifying a motion platform:
velocity vector V under the geographic system obtained by resolving in step twokAnd the velocity vector of the sky
Figure BDA0003520137820000121
The module value of (2) is detected and classified by a motion platform:
the escalator detection formula is as follows:
Figure BDA0003520137820000122
wherein, Tescal,normIndicating the speed threshold, T, of the escalatorescal,URepresenting a speed threshold of the escalator in the direction of the sky;
the vertical elevator detection formula is as follows:
Figure BDA0003520137820000123
then the pedestrian is positioned in the vertical elevator at the moment k and the elevator starts to run; t isele,normIndicating the speed threshold of the vertical elevator, Tele,URepresenting the vertical elevator speed threshold in the direction of the day.
(2) Detecting the existence of a certain type of motion platform, synchronously carrying out motion sub-state classification of the certain type of platform, wherein a sub-state classification model is as follows:
(2.1) according to the above behavior example, the whole moving state of the escalator is divided into five sub-states S11-S15 according to different directions of the speed vectors of the steps in the moving process of the escalator, and the five sub-states are switched in a fixed sequence (S11-S12-S13-S14-S15), as shown in the following fig. 2:
a sub-state S11 indicating a pedestrian at the entrance of the escalator; the elevator has only forward speed and the speed vector is constant;
a sub-state S12 indicating a pedestrian is in a corner position near the entrance of the escalator; the forward speed of the elevator is reduced, the speed in the vertical direction is increased, the speed module value is constant, and the value is consistent with the sub-state S11;
a sub-state S13 indicating a pedestrian positioned on the escalator moving upwardly with the escalator; the elevator has a forward speed and a vertical speed, the speed vector is constant, and the speed module value is consistent with the sub-state S11;
a sub-state S14 indicating a pedestrian is in a corner position near the exit of the escalator; the forward speed of the elevator is increased, the speed in the vertical direction is reduced, and the speed module value is consistent with the sub-state S11;
in the sub-state S15, the pedestrian is at the exit of the escalator, the elevator has only forward speed, and the speed vector is constant, and the velocity modulus is consistent with the sub-state S11.
(2.2) according to different speed vector module value change rules in the running process of the vertical elevator, dividing the whole motion state of the vertical elevator into three sub-states S21-S23, and switching the three sub-states in a fixed sequence (S21-S22-S23) as shown in the following figure 3. Meanwhile, in the running process of the vertical elevator, only the speed in the vertical direction is 0, and the speed in the horizontal direction is 0;
a sub-state S21, which indicates that the vertical elevator is moving upward with an acceleration, and the vertical speed is gradually increased from 0 to a fixed value;
a sub-state S22, which indicates that the vertical elevator is moving upward at a constant speed and the vertical speed is constant;
sub-state S23, indicating that the vertical elevator is traveling upwards at a reduced speed and the vertical speed is decreasing to 0.
(3) From the above analysis, the forward search tag Count is obtained by comparing the positive and negative of the absolute acceleration in the vertical direction in the sliding windowk,fpAnd judging whether the motion sub-state switching condition is satisfied. Specific sub-state classification algorithms are applied to different platforms:
Figure BDA0003520137820000131
wherein, Countk,fpRepresenting a forward search tag, N representing the length of a sliding window, sgn () representing a sign function, returning 1 if the parameter is positive, returning 0 if the parameter is 0, and returning if the parameter is negative; a. theURepresents the output of the acceleration specific force in the direction of the acceleration,
Figure BDA0003520137820000132
g is the local gravitational acceleration value.
When the moving platform is an escalator, taking the behavior example on the escalator:
when Count k,fp1 denotes that at time k, the substate switches to S11；
When 0.9N is less than or equal to Countk,fpN ≦ N, meaning that at time k, the substate is from S11Switching to S12；
when-0.2N is less than or equal to Countk,fp≦ 0.2N, meaning that at time k, the substate is from S12Switching to S13；
when-N is less than or equal to Countk,fpLess than or equal to-0.9N, which indicates that at the time k, the substate is S13Switching to S14；
when-0.2N is less than or equal to Countk,fp≦ 0.2N, meaning that at time k, the substate is from S14Switching to S15；
When the motion platform is a vertical elevator, taking the vertical elevator to ascend as an example:
when 0.9N is less than or equal to Countk,fpN is less than or equal to the sum of the values of the sub-states of the pedestrian and the S, and the pedestrian is in the vertical elevator and starts to move at the moment k21；
when-0.2N is less than or equal to Countk,fp≦ 0.2N, meaning that at time k, the substate is from S21Switching to S22；
when-N is less than or equal to Countk,fp≦ 0.9N, meaning that at time k, the substate is from S22Switching to S23。
In conclusion, the time interval of each motion sub-state of the vertical elevator and the escalator can be judged.
(4) Setting corresponding speed constraints based on the sub-state classification:
according to the motion characteristics of different sub-states in the motion process of the escalator and the vertical elevator, different speed constraint methods are set, and the following table shows that:
Figure BDA0003520137820000141
the lateral zero-speed correction method, the horizontal zero-speed correction method and the vertical zero-speed correction method are all low-dimensional representations of the zero-speed correction method in the fourth step, and the specific formula is as follows:
Zzupt,lateral＝δVlateral＝VE-0＝HlateralX+vlateral。
Zzupt,horiz＝δVhoriz＝[VE VN]T-[0 0]T＝HhorizX+vhoriz
Zzupt,vert＝δVvert＝VU-0＝HvertX+vvert
wherein Z iszupt,lateralObservation quantity Z representing lateral zero-speed correctionzupt_horizObservation quantity Z representing horizontal zero-speed correctionzupt,vertRepresents the observed quantity of the vertical zero-speed correction; delta VlateralIMU velocity error quantity, delta V, representing lateral zero velocity correctionhorizIMU speed error quantity, delta V, representing horizontal zero speed correctionvertAn IMU velocity error amount representing a vertical velocity correction; vEVelocity vector V representing east directionNVelocity vector, V, representing northUA velocity vector representing a direction of the day; hlateral＝[01×3 1 01×14]，Hhoriz＝[02×3 I2×2 02×13]，Hvert＝[01×5 1 01×12]；vlateralObservation noise matrix, v, representing lateral zero-velocity correctionhorizObservation noise matrix, v, representing lateral zero-velocity correctionvertAn observed noise matrix representing a vertical zero-speed correction.
The formula of the constant speed modulus correction method is as follows:
Figure BDA0003520137820000142
Figure BDA0003520137820000143
Zcons_norm＝δVk,norm＝Vk,norm-Vob,norm＝f(Vk)-Vob,norm+nn
wherein, T11And T12Indicating entry into substate S11And S12The time of (d); vob,normRepresenting a kinematic sub-state S11Mean value, V, of velocity vector modulus obtained by solution in the processk,normRepresenting the modulus value of the velocity vector at time k,
Figure BDA0003520137820000151
a velocity vector indicating the east direction at the time k,
Figure BDA0003520137820000152
A velocity vector representing the north direction at time k,
Figure BDA0003520137820000153
A velocity vector representing the direction of the day at time k; f () represents a modulo function, Zcons_normIndicating the observed quantity, deltaV, of constant correction of the velocity mode valuek,normRepresenting the velocity module error, n, of the fully bound IMU at time knTo observe the noise matrix;
and (3) performing Taylor series expansion on the nonlinear equation of the formula and reserving a linear term to obtain a linear observation error measurement equation:
Zcons_n＝δVk,norm＝JX+nn
wherein J may represent a Jacobian matrix of the nonlinear function with respect to the system state vector X, and the formula is
Figure BDA0003520137820000154
The forward and vertical direction velocity vector constant correction method observation equation formula is as follows:
Figure BDA0003520137820000155
wherein Z iscons_vectRepresenting the velocity error observed quantity of velocity vector constant correction in the forward direction and the vertical direction; delta Vk,vectRepresents the difference between the east and north velocity vectors at time k and time k-1, nvTo observe the noise matrix; hcons_vect＝[02×3 I2×202×13]。
Correcting the speed, the attitude and the position of the pedestrian by applying an extended Kalman filtering method according to the system equation and the measurement equation;
Figure BDA0003520137820000156
Figure BDA0003520137820000157
Figure BDA0003520137820000158
Figure BDA0003520137820000159
wherein A represents a state transition matrix, Q represents a state transition covariance matrix,
Figure BDA00035201378200001510
representing the prior estimated covariance, P, of time kk-1Representing the posteriori estimated covariance, K, at time K-1kRepresenting the Kalman filter gain, J representing the Jacobian matrix, R representing the measurement noise covariance, Z representing the observed quantity,
Figure BDA00035201378200001511
representing a prior estimate of the state at time k, X, from the system equationkMaximum solved by Kalman filterThe method has the advantages of excellent estimation,
Figure BDA0003520137820000161
representing the posterior estimation covariance at the time k, and I representing an identity matrix;
solving the optimal estimation error state quantity XkThereafter, the position, velocity, attitude and IMU device errors are corrected again.
The experimental scene of this embodiment selects for use teaching experiment building, is escalator and vertical elevator experiment respectively, and the total length of route is 41.5m and 63.2m respectively.
As shown in fig. 4, in the escalator experiment, the tester moves forward with a walk of 1.5m/s, and normally rides when meeting the escalator without stopping in the moving process. In the process of taking the elevator, a navigation resolving result is shown by a dotted line by using a zero-speed correction algorithm only during walking, an inertial navigation error is rapidly dispersed, and a starting point error is more than 20 meters; the calculation result of the pedestrian inertial navigation method suitable for the moving platform scene is shown by the solid line in the figure, the starting and ending point position error is 0.53 m, the horizontal and height positioning errors are greatly reduced, and the positioning precision is effectively improved.
As shown in fig. 5, and in the vertical elevator experiment, in a building with a total height of 10 floors, the tester experiment was initially stationary in a 1-floor elevator, followed by 8 stops at random floors, and finally stopped at 2 floors. The result obtained by the navigation method without using the velocity error of the moving platform is shown by a dotted line, the inertial navigation error rapidly diverges, and the starting and ending point error is more than 100 meters; the result solved by the pedestrian inertial navigation method applicable to the moving platform scene is shown by a solid line in the figure, the starting and ending point position error is 1.7 meters, the height positioning error is greatly reduced, and the positioning precision is effectively improved.

Claims (10)

1. A pedestrian inertial navigation method suitable for a motion platform scene is characterized by comprising the following steps:
(1) acquiring pedestrian foot binding type IMU data, and estimating a zero-speed detection threshold value through initial static data;
(2) preprocessing the acquired foot-bound IMU data and resolving the speed, the attitude and the position in real time by adopting a strapdown inertial navigation method;
(3) establishing a gait detection model of four nodes of the leg and the foot, and performing zero-speed detection by using an estimated zero-speed detection threshold;
(4) when the zero-speed detection result is in a zero-speed state, performing zero-speed correction by using a Kalman filter to obtain the corrected speed, posture and position, and performing error correction on the IMU device; otherwise, no correction is carried out;
(5) comparing the related speed obtained by resolving in the step (2) with a set threshold value to judge whether a motion platform and the type of the motion platform exist at the current moment; when the existence of a certain motion platform is detected, sub-states of the motion of the certain motion platform are synchronously classified, different speed constraints are applied according to the motion characteristics of different sub-states, and the speed, the posture and the position of a pedestrian are corrected by using a filtering method; and if the motion platform is not detected, repeating the steps (2) - (5).
2. The pedestrian inertial navigation method applicable to the moving platform scene according to claim 1, wherein the moving platform detection and classification in step (5) is specifically:
if the following conditions are met:
Figure FDA0003520137810000011
then the pedestrian is positioned at the escalator at the moment k; wherein, VkRepresents a velocity vector in the geographic region,
Figure FDA0003520137810000012
representing the velocity vector, T, in the direction of the skyescal,normIndicating the speed threshold, T, of the escalatorescal,URepresenting a direction-of-day speed threshold of the escalator;
if the following conditions are met:
Figure FDA0003520137810000013
then the pedestrian is positioned in the vertical elevator at the moment k and the elevator starts to run; wherein, VkRepresenting a velocity vector in a geographic region,
Figure FDA0003520137810000014
representing the velocity vector, T, in the direction of the skyele,normIndicating the speed threshold of the vertical elevator, Tele,URepresenting the vertical elevator speed threshold in the direction of the day.
3. The pedestrian inertial navigation method of claim 2, characterized in that,
when the escalator is positioned at the current moment, the whole-course motion state of the escalator is divided into five sub-states S11-S15, and the five sub-states are switched in a fixed sequence; wherein, substate S11 shows that the pedestrian is in the staircase entry, adopts the speed restraint method to be: firstly, performing lateral zero-speed correction and then performing vertical zero-speed correction; the sub-state S12 represents a pedestrian positioned at a corner near the entrance of the escalator, using the speed constraint method: firstly, performing lateral zero-speed correction and then performing constant speed modulus correction; the substate S13 shows the pedestrian is on the escalator, using the speed constraint method: firstly, performing lateral zero-speed correction and then performing forward and vertical constant-speed correction; sub-state S14 represents a corner position near the exit of the escalator, using the speed constraint method: firstly, performing lateral zero-speed correction and then performing constant speed modulus correction; the sub-state S11 represents a pedestrian at the exit of the escalator, using the speed constraint method: firstly, performing lateral zero-speed correction and then performing vertical zero-speed correction;
when the current time is in the vertical elevator, the whole-course motion state of the vertical elevator is divided into three sub-states S21-S23, and the three sub-states are switched in a fixed sequence; substate S21 represents a vertical elevator acceleration traveling upwards, and the method of using the speed constraint is: carrying out horizontal zero-speed correction; substate S22 represents a constant speed up run of the vertical elevator, the method of using speed constraints being: firstly, carrying out horizontal zero-speed correction and then carrying out vertical constant-speed correction; substate S23 represents a vertical elevator traveling at reduced speed and upward, and the method of using speed constraints is: and carrying out horizontal zero-speed correction.
4. The pedestrian inertial navigation method applicable to the moving platform scene according to claim 3, wherein the lateral zero velocity correction method, the horizontal zero velocity correction method and the vertical zero velocity correction method are all low-dimensional representations of the zero velocity correction method, and a specific formula is as follows:
Zzupt,lateral＝δVlateral＝VE-0＝HlateralX+vlateral
Zzupt,horiz＝δVhoriz＝[VE VN]T-[0 0]T＝HhorizX+vhoriz
Zzupt,vert＝δVvert＝VU-0＝HvertX+vvert
wherein Z iszupt,lateralObservation quantity Z representing lateral zero-speed correctionzupt_horizObservation quantity Z representing horizontal zero-speed correctionzupt,vertRepresents the observed quantity of the vertical zero-speed correction; delta VlateralIMU velocity error quantity, delta V, representing lateral zero velocity correctionhorizIMU speed error quantity, delta V, representing horizontal zero speed correctionvertAn IMU velocity error amount representing a vertical velocity correction; vEVelocity vector V representing east directionNVelocity vector, V, representing northUA velocity vector representing a direction of the day; hlateral＝[01×3 1 01×14]，Hhoriz＝[02×3 I2×2 02×13]，Hvert＝[01×5 1 01×12]；vlateralObservation noise matrix, v, representing lateral zero-velocity correctionhorizObservation noise matrix, v, representing lateral zero-velocity correctionvertAn observed noise matrix representing a vertical zero velocity correction;
the formula of the constant speed modulus correction method is as follows:
Figure FDA0003520137810000031
Figure FDA0003520137810000032
Zcons_norm＝δVk,norm＝Vk,norm-Vob,norm＝f(Vk)-Vob,norm+nn
wherein, T11And T12Indicating entry into substate S11And S12The time of (d); vob,normRepresenting a kinematic sub-state S11Mean value, V, of velocity vector modulus obtained by solution in the processk,normRepresenting the modulus value of the velocity vector at time k,
Figure FDA0003520137810000033
a velocity vector indicating the east direction at the time k,
Figure FDA0003520137810000034
A velocity vector representing the north direction at time k,
Figure FDA0003520137810000035
A velocity vector representing the direction of the day at time k; f () represents a modulo function, Zcons_normIndicating the observed quantity, deltaV, of constant correction of the velocity mode valuek,normRepresenting the velocity modulus error, n, of the fully bound IMU at time knTo observe the noise matrix;
the forward and vertical direction velocity vector constant correction method observation equation formula is as follows:
Figure FDA0003520137810000036
wherein, Zcons_vectRepresenting the velocity error observed quantity of velocity vector constant correction in the forward direction and the vertical direction; delta Vk,vectEast and north velocity vector representing time kDifference between the quantity and the time k-1, nvTo observe the noise matrix; hcons_vect＝[02×3 I2×2 02×13]。
5. The method for inertial navigation of pedestrians in a moving platform scene as claimed in claim 3, wherein the classification algorithm formula of the sub-states is:
Figure FDA0003520137810000037
wherein, Countk,fpDenotes the forward search tag, N denotes the sliding window length, sgn () denotes the sign function, AURepresenting the output of the acceleration specific force in the sky direction, wherein g is a local gravity acceleration value;
when the moving platform is an escalator, the moving platform is a ladder,
when Countk,fp1 denotes that at time k, the substate switches to S11；
When 0.9N is less than or equal to Countk,fpN ≦ N, meaning that at time k, the substate is from S11Switching to S12；
when-0.2N is less than or equal to Countk,fp≦ 0.2N, meaning that at time k, the substate is from S12Switching to S13；
when-N is less than or equal to Countk,fp≦ 0.9N, meaning that at time k, the substate is from S13Switching to S14；
when-0.2N is less than or equal to Countk,fp≦ 0.2N, meaning that at time k, the substate is from S14Switching to S15；
When the moving platform is a vertical elevator,
when 0.9N is less than or equal to Countk,fpN is less than or equal to the sum of the values of the sub-states of the pedestrian and the S, and the pedestrian is in the vertical elevator and starts to move at the moment k21；
when-0.2N is less than or equal to Countk,fp≦ 0.2N, meaning that at time k, the substate is from S21Switching to S22；
when-N is less than or equal to Countk,fp≦ 0.9N, meaning at time k, child-likeState by S22Switching to S23。
6. The pedestrian inertial navigation method applicable to the moving platform scene according to claim 1, wherein the zero-speed detection threshold estimation in step (1) is specifically: the center of the foot-bound inertial sensor is taken as a coordinate origin O, the right direction is taken as an X axis, a Y axis is perpendicular to the X axis and points to the front direction, and a Z axis points to the upper direction; acquiring pedestrian foot binding type IMU data, and acquiring a zero-speed detection threshold value according to the data characteristics of acceleration and angular speed in an initial static interval; the detection threshold comprises a minimum threshold T of an acceleration modulus valueA,minMaximum threshold value T of acceleration modulusA,maxAcceleration standard deviation threshold
Figure FDA0003520137810000041
Threshold value T of angular velocity modulusWAngular velocity standard deviation threshold
Figure FDA0003520137810000042
The calculation formula is as follows:
Figure FDA0003520137810000043
wherein A isstaicIs the mean, σ, of the acceleration mode in the static intervalA_staticIs the standard deviation, W, of the acceleration mode in the static intervalstaicIs the mean, σ, of the angular velocity mode in the static intervalW_staticThe standard deviation of the angular velocity mode in the static interval; c. CA,minExpressed as the minimum constant coefficient of the acceleration modulus, cA,maxExpressed as the maximum constant coefficient of the acceleration modulus,
Figure FDA0003520137810000044
expressed as the standard deviation of acceleration,
Figure FDA0003520137810000045
Expressed as the modulus of angular velocity,cWExpressed as a constant coefficient of standard deviation.
7. The pedestrian inertial navigation method applicable to the moving platform scene according to claim 1, wherein the step (2) specifically comprises the following steps:
(2.1) calculating and deducting accelerometer zero-bias epsilon from IMU static databAnd zero bias of the gyroscopeb；
(2.2) calculating an initial attitude angle by using the specific force output of the accelerometer under the static state, wherein the initial attitude angle comprises a pitch angle theta and a roll angle gamma:
Figure FDA0003520137810000051
Figure FDA0003520137810000052
wherein, gxOutputting specific force of an X axis of the static lower foot binding type IMU; gySpecific force output of the Y axis of the static lower foot binding type IMU; g is a radical of formulazOutputting specific force of a Z axis of the static lower foot binding type IMU;
(2.3) resolving the attitude, speed and position of the pedestrian in real time
The posture updating algorithm formula of the foot-bound IMU is as follows:
Figure FDA0003520137810000053
wherein Q represents a quaternion matrix, tk-1Denotes the time k-1, tkRepresents the next sampling instant of time k, i.e., time k-1; ψ represents a heading angle, θ represents a pitch angle, γ represents a roll angle,
Figure FDA0003520137810000054
representing a gesture;
the velocity update algorithm formula of the foot-bound IMU is as follows:
Figure FDA0003520137810000055
wherein, VkIs a motion velocity vector V in the geographic system at the moment kk-1The motion velocity vector is the motion velocity vector under the geographic system at the moment k-1;
Figure FDA0003520137810000056
a coordinate transformation matrix representing the machine system to the navigation system,
Figure FDA0003520137810000057
represents specific force, gnRepresents the local gravitational acceleration, Δ t represents the sampling time interval;
the position updating algorithm formula of the foot-bound IMU is as follows:
Figure FDA0003520137810000058
wherein L represents longitude, λ represents latitude, and h represents altitude; r ismRepresents the radius of curvature, R, in the meridian plane of the earthnAnd (3) representing the inner curvature radius of the earth prime plane.
8. The pedestrian inertial navigation method applicable to the moving platform scene according to claim 1, wherein in the step (3), the gait detection models of the four nodes of the leg and the foot are:
Figure FDA0003520137810000059
Figure FDA0003520137810000061
Figure FDA0003520137810000062
Figure FDA0003520137810000063
ZVD(k)＝S1(k)&S2(k)&S3(k)&S4(k)
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003520137810000064
represents the mean value, σ, of the accelerations at the k-th to k + N momentsA,k:k+nRepresents the standard deviation of the acceleration at the k-th to k + N-th time points,
Figure FDA0003520137810000065
Represents the mean value, σ, of the angular velocities at the k-th to k + N-th momentsW,k:k+NRepresents the standard deviation of the angular velocity in the k-th to k + N-th time instants; t isA,minIs the minimum threshold value, T, of the accelerationA,maxIs a maximum threshold value of the acceleration,
Figure FDA0003520137810000066
Is a threshold value, T, of the standard deviation of the accelerationWIs a threshold value of angular velocity,
Figure FDA0003520137810000067
A threshold value of the standard deviation of angular velocity; when S is1(k)、S2(k)、S3(k)、S4(k) At the same time, when 1, zvd (k) is 1, which indicates that the zero speed state is currently set.
9. The pedestrian inertial navigation method applicable to the moving platform scene according to claim 1, wherein the step (4) specifically comprises the following steps:
(4.1) when the detected foot is in a zero-speed state, performing error estimation by using Kalman filtering calculation to construct an 18-dimensional Kalman filter state quantity X:
Figure FDA0003520137810000068
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003520137810000069
respectively representing platform error angles of east, north and sky directions; δ V ═ δ VE δVN δVU]TSpeed errors in the east, north and sky directions respectively; δ P ═ δ L δ λ δ h]TRespectively representing errors of longitude, latitude and altitude;
Figure FDA00035201378100000610
respectively representing zero offset errors of an X axis, a Y axis and a Z axis of the gyroscope;
Figure FDA00035201378100000611
respectively representing first-order Markov drift errors of an X axis, a Y axis and a Z axis of the gyroscope;
Figure FDA00035201378100000612
respectively representing first-order Markov drift errors of an X axis, a Y axis and a Z axis of the accelerometer;
(4.2) establishing a state equation:
Figure FDA00035201378100000613
wherein A represents a state transition matrix, G represents a system noise transition matrix, and W represents system noise;
(4.3) establishing a measurement equation:
Zzupt＝δV＝VINS-VZUPT＝HX+v
wherein Z iszuptRepresenting the observed quantity, i.e. IMU speed error quantity delta V, VINSIMU velocity vector, V, resolved for strapdown inertial navigationZUPTTheoretical value of [ 000]TH is an observation matrix, v is an observation noise matrix and represents the uncertainty of speed observation;
and (4.4) correcting the navigation error and correcting the IMU device error:
constructing a Kalman filter according to the steps (4.1), (4.2) and (4.3), and solving to obtain the optimal estimation state quantity
Figure FDA0003520137810000071
Deducing the former nine-dimensional state quantity from the strapdown inertial navigation resolving result in the step (2)
Figure FDA0003520137810000072
Obtaining the corrected attitude, speed and position, and deducting the nine-dimensional state quantity from the IMU data
Figure FDA0003520137810000073
And correcting the zero offset error and the first-order Markov drift error of the IMU.
10. The pedestrian inertial navigation method applicable to the moving platform scene according to claim 1, wherein the speed, the attitude and the position of the pedestrian are corrected by using a filtering method in the step (5);
Figure FDA0003520137810000074
Figure FDA0003520137810000075
Figure FDA0003520137810000076
Figure FDA0003520137810000077
wherein A represents a state transition matrix, Q represents a state transition covariance matrix,
Figure FDA0003520137810000078
representing the prior estimated covariance, P, of time kk-1Representing the posteriori estimated covariance, K, at time K-1kRepresenting the Kalman filter gain, J representing the Jacobian matrix, R representing the measurement noise covariance, Z representing the observed quantity,
Figure FDA0003520137810000079
representing a prior estimate of the state at time k, X, from the system equationkThe optimal estimate obtained by the Kalman filter solution,
Figure FDA00035201378100000710
representing the posterior estimation covariance at the time k, and I representing an identity matrix;
solving the optimal estimation error state quantity XkThereafter, the position, velocity, attitude and IMU device errors are corrected again.
CN202210180015.5A 2022-02-25 2022-02-25 Pedestrian inertial navigation method suitable for motion platform scene Pending CN114608571A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210180015.5A CN114608571A (en) 2022-02-25 2022-02-25 Pedestrian inertial navigation method suitable for motion platform scene

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210180015.5A CN114608571A (en) 2022-02-25 2022-02-25 Pedestrian inertial navigation method suitable for motion platform scene

Publications (1)

Publication Number Publication Date
CN114608571A true CN114608571A (en) 2022-06-10

Family

ID=81858887

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210180015.5A Pending CN114608571A (en) 2022-02-25 2022-02-25 Pedestrian inertial navigation method suitable for motion platform scene

Country Status (1)

Country Link
CN (1) CN114608571A (en)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109297485A (en) * 2018-08-24 2019-02-01 北京航空航天大学 A kind of personal inertial navigation height accuracy method for improving in interior based on height from observation algorithm
CN111024126A (en) * 2019-12-26 2020-04-17 北京航天控制仪器研究所 Self-adaptive zero-speed correction method in pedestrian navigation positioning
CN112066980A (en) * 2020-08-31 2020-12-11 南京航空航天大学 Pedestrian navigation positioning method based on human body four-node motion constraint
CN113029139A (en) * 2021-04-07 2021-06-25 中国电子科技集团公司第二十八研究所 Airport flight area vehicle differential Beidou/SINS combined navigation method based on motion detection
CN113295158A (en) * 2021-05-14 2021-08-24 江苏大学 Indoor positioning method fusing inertial data, map information and pedestrian motion state

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109297485A (en) * 2018-08-24 2019-02-01 北京航空航天大学 A kind of personal inertial navigation height accuracy method for improving in interior based on height from observation algorithm
CN111024126A (en) * 2019-12-26 2020-04-17 北京航天控制仪器研究所 Self-adaptive zero-speed correction method in pedestrian navigation positioning
CN112066980A (en) * 2020-08-31 2020-12-11 南京航空航天大学 Pedestrian navigation positioning method based on human body four-node motion constraint
CN113029139A (en) * 2021-04-07 2021-06-25 中国电子科技集团公司第二十八研究所 Airport flight area vehicle differential Beidou/SINS combined navigation method based on motion detection
CN113295158A (en) * 2021-05-14 2021-08-24 江苏大学 Indoor positioning method fusing inertial data, map information and pedestrian motion state

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张苗，等: "基于人体运动学辅助的可穿戴式行人导航系统", 导航与控制, vol. 17, no. 4, 31 August 2018 (2018-08-31), pages 1 - 6 *

Similar Documents

Publication Publication Date Title
Tong et al. A double-step unscented Kalman filter and HMM-based zero-velocity update for pedestrian dead reckoning using MEMS sensors
CN109827577B (en) High-precision inertial navigation positioning algorithm based on motion state detection
CN110118560B (en) Indoor positioning method based on LSTM and multi-sensor fusion
CN113945206A (en) Positioning method and device based on multi-sensor fusion
CN110207692B (en) Map-assisted inertial pre-integration pedestrian navigation method
EP3191795A1 (en) Method and apparatus for using map information aided enhanced portable navigation
CN111136660A (en) Robot pose positioning method and system
US20130110397A1 (en) Method and System for Detection of a Zero Velocity State of an Object
CN111024126B (en) Self-adaptive zero-speed correction method in pedestrian navigation positioning
CN109612463B (en) Pedestrian navigation positioning method based on lateral speed constraint optimization
CN112965063B (en) Robot mapping and positioning method
CN108426582B (en) Indoor three-dimensional map matching method for pedestrians
CN110672095A (en) Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation
CN111649742B (en) Elevation estimation method based on ANFIS assistance
CN113295158A (en) Indoor positioning method fusing inertial data, map information and pedestrian motion state
CN109708630B (en) Step strapdown height measurement method based on SHE model
CN114485643B (en) Coal mine underground mobile robot environment sensing and high-precision positioning method
CN112985392B (en) Pedestrian inertial navigation method and device based on graph optimization framework
Kronenwett et al. Elevator and escalator classification for precise indoor localization
CN116448103A (en) Pedestrian foot binding type inertial navigation system error correction method based on UWB ranging assistance
CN114608571A (en) Pedestrian inertial navigation method suitable for motion platform scene
CN113483753B (en) Inertial course error elimination method based on environmental constraint
CN111912406B (en) Indoor pedestrian navigation course correction method and system based on improved HDE
Liu et al. GPS/INS integrated navigation with LSTM neural network
Wang et al. Indoor trajectory identification: Snapping with uncertainty

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