A pedestrian inertial navigation method suitable for motion platform scenarios

Abstract

The present invention discloses a pedestrian inertial navigation method suitable for a motion platform scene, comprising the following steps: (1) collecting pedestrian foot-bound IMU data and estimating a zero-speed detection threshold; (2) pre-processing data and performing real-time calculation of speed, posture and position; (3) establishing a gait detection model and performing zero-speed detection using the estimated zero-speed detection threshold; (4) performing zero-speed correction using a Kalman filter when in a zero-speed state; otherwise, no correction is performed; (5) comparing the relevant speed obtained by calculating the speed in step (2) with a set threshold to determine whether a motion platform exists at the current moment and the type of the motion platform; when a certain type of motion platform is detected, classifying the motion sub-states of the platform, applying different speed constraints, and using a filtering method to correct the pedestrian speed, posture and position; if no motion platform is detected, repeating steps (2)-(5). The present invention solves the problem of inaccurate positioning of pedestrian inertial navigation in a motion platform scene.

Description

Technical Field

[0001] The present 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 Art

[0002] Indoor pedestrian positioning service is the core supporting technology in the fields of Internet of Things (IoT), big data, artificial intelligence (AI), etc. With the vigorous development of these technologies, the importance of indoor positioning has become increasingly prominent. With the rapid development and popularization of MEMS technology, pedestrian navigation systems based on MEMS inertial sensors have been realized. The common pedestrian inertial navigation system is a kind of inertial sensor fixed to the foot of pedestrians. Based on the traditional SINS algorithm, the system combines the unique gait characteristics of foot movement and uses the zero velocity update (ZUPT) method to periodically correct navigation errors. Therefore, zero velocity correction occupies a vital position in the inertial navigation system. It is an effective means to constrain the divergence of inertial navigation errors.

[0003] The current pedestrian inertial navigation method is only studied for static ground scenes. However, when pedestrians are in high-rise buildings such as office buildings and shopping malls, or take external motion platforms such as vertical elevators or escalators, the zero-speed detection algorithm often misdetects the zero-speed state, resulting in incorrect speed observations, and the speed and position errors of the system will diverge rapidly. Therefore, in the external motion platform scene, if there is no speed error constraint or the speed error constraint is incorrect, it is impossible to meet the needs of high-precision and reliable pedestrian positioning and navigation. Summary of the invention

[0004] Purpose of the invention: In view of the above shortcomings, the present invention provides a pedestrian inertial navigation method suitable for motion platform scenarios, which solves the problem of inaccurate pedestrian inertial navigation positioning in motion platform scenarios, reduces the cumulative error of pedestrian navigation positioning, and improves the navigation positioning accuracy.

[0005] Technical solution: To solve the above problems, the present invention provides a pedestrian inertial navigation method suitable for a motion platform scene, comprising the following steps:

[0006] (1) Collect pedestrian foot-mounted IMU data and estimate the zero-speed detection threshold based on the initial static data;

[0007] (2) Preprocess the collected foot-worn IMU data and use the strapdown inertial navigation method to perform real-time calculation of speed, attitude, and position;

[0008] (3) Establish a four-node gait detection model for the legs and feet, and use the estimated zero-speed detection threshold to perform zero-speed detection;

[0009] (4) When the zero-speed detection result is in the zero-speed state, the Kalman filter is used to perform zero-speed correction to obtain the corrected speed, attitude and position, and perform error correction on the IMU device; otherwise, no correction is performed;

[0010] (5) The relevant speed calculated according to step (2) is compared with the set threshold to determine whether there is a motion platform at the current moment and the type of the motion platform; when a certain type of motion platform is detected, the motion sub-states of the platform are simultaneously classified, different speed constraints are imposed according to the motion characteristics of different sub-states, and the pedestrian speed, posture and position are corrected using a filtering method; if no motion platform is detected, steps (2)-(5) are repeated.

[0011] Furthermore, the motion platform detection and classification described in step (5) is specifically as follows:

[0012] If satisfied:

[0013]

[0014] It means that the pedestrian is on the escalator at time k; where V k represents the velocity vector in the geographic system, represents the celestial velocity vector, T escal,norm represents the speed threshold of the escalator, T escal,U Indicates the escalator's celestial speed threshold;

[0015] If satisfied:

[0016]

[0017] It means that at time k, the pedestrian is in the vertical elevator and the elevator starts running; where V k represents the velocity vector in the geographic system, represents the celestial velocity vector, T ele,norm represents the speed threshold of the vertical elevator, T ele,U Indicates the celestial velocity threshold of the vertical elevator.

[0018] Furthermore, if the pedestrian is currently on the escalator, the entire motion state of the escalator is divided into five sub-states S11-S15, and the five sub-states are switched in a fixed order; wherein, sub-state S11 indicates that the pedestrian is at the entrance of the escalator, and the speed constraint method is: first perform a lateral zero-speed correction and then perform a vertical zero-speed correction; sub-state S12 indicates that the pedestrian is at a turning position close to the entrance of the escalator, and the speed constraint method is: first perform a lateral zero-speed correction and then perform a speed modulus constant correction; sub-state S13 indicates that the pedestrian is on the escalator, and the speed constraint method is: first perform a lateral zero-speed correction and then perform a forward and vertical constant speed correction; sub-state S14 indicates that the pedestrian is at a turning position close to the exit of the escalator, and the speed constraint method is: first perform a lateral zero-speed correction and then perform a speed modulus constant correction; sub-state S15 indicates that the pedestrian is at the exit of the escalator, and the speed constraint method is: first perform a lateral zero-speed correction and then perform a vertical zero-speed correction;

[0019] If the elevator is in a vertical position at the current moment, the entire motion state of the vertical elevator is divided into three sub-states S21-S23, and the five sub-states are switched in a fixed order; sub-state S21 indicates that the vertical elevator is accelerating upward, and the method of using speed constraint is: performing horizontal zero-speed correction; sub-state S22 indicates that the vertical elevator is running at a constant speed upward, and the method of using speed constraint is: first performing horizontal zero-speed correction and then performing vertical constant-speed correction; sub-state S23 indicates that the vertical elevator is decelerating upward, and the method of using speed constraint is: performing horizontal zero-speed correction.

[0020] Furthermore, the lateral zero-speed correction, horizontal zero-speed correction, and vertical zero-speed correction methods are all low-dimensional representations of the zero-speed correction method, and the specific formulas are:

[0021] Z zupt,lateral =δV lateral =V E -0=H lateral X+v lateral .

[0022] Z zupt,horiz =δV horiz =[V E V N ] T -[0 0] T =H horiz X+v horiz

[0023] Z zupt,vert =δV vert =V U -0=H vert X+v vert

[0024] Among them, Zzupt,lateral represents the observed value of lateral zero-speed correction, Z zupt_horiz Represents the observed value of horizontal zero-speed correction, Z zupt,vert Represents the observed value of vertical zero-speed correction; δV lateral Indicates the IMU velocity error of the lateral zero-speed correction, δV horiz Indicates the IMU velocity error of horizontal zero speed correction, δV vert Indicates the IMU velocity error of vertical velocity correction; V E represents the velocity vector in the east direction, V N represents the velocity vector in the north direction, V U H represents the velocity vector in the celestial direction; lateral =[0 1×3 1 0 1×14 ], H horiz =[0 2×3 I 2×2 0 2×13 ], H vert =[0 1×5 1 0 1×12 ];v lateral represents the observation noise matrix of lateral zero-speed correction, v horiz represents the observation noise matrix of lateral zero-speed correction, v vert represents the observation noise matrix of vertical zero-speed correction;

[0025] The speed modulus constant correction method formula is as follows:

[0026]

[0027]

[0028] Z cons_norm =δV k,norm =V k,norm -V ob,norm =f(V k )-V ob,norm +n n

[0029] Among them, T 11 and T 12 Indicates entering substate S 11 and S 12 Time; V ob,norm Represents the motion substate S 11 The mean value of the velocity vector modulus obtained during the process, V k,norm represents the modulus of the velocity vector at time k, represents the eastward velocity vector at time k, represents the velocity vector to the north at time k, represents the velocity vector in the celestial direction at time k; f( ) represents the modulus function, Z cons_norm represents the observed value of the constant correction of the velocity modulus, δV k,norm represents the velocity modulus error of the foot-bound IMU at time k, n n is the observation noise matrix;

[0030] The observation equation formula of the forward and vertical velocity vector constant correction method is as follows:

[0031]

[0032] Among them, Z cons_vect The velocity error observation that represents the constant correction of the forward and vertical velocity vector; δV k,vect represents the difference between the east and north velocity vectors at time k and time k-1, n v is the observation noise matrix; H cons_vect =[0 2×3 I 2×2 0 2×13 ].

[0033] Furthermore, the classification algorithm formula of the sub-state is:

[0034]

[0035] Among them, Count k,fp represents the forward search label, N represents the sliding window length, sgn() represents the sign function, A U It indicates the acceleration azimuth specific force output, where g is the local gravity acceleration value;

[0036] When the moving platform is an escalator,

[0037] When Count k,fp =1, indicating that at time k, the substate switches to S 11 ;

[0038] When 0.9N≤Count k,fp ≤N, indicating that at time k, the substate is S 11 Switch to S 12 ;

[0039] When -0.2N≤Count k,fp ≤0.2N, indicating that at time k, the substate is S 12 Switch to S 13 ;

[0040] When -N≤Count k,fp ≤-0.9N, indicating that at time k, the substate is S 13 Switch to S 14 ;

[0041] When -0.2N≤Count k,fp ≤0.2N, indicating that at time k, the substate is S 14 Switch to S 15 ;

[0042] When the motion platform is a vertical elevator,

[0043] When 0.9N≤Count k,fp ≤N, indicating that at time k, the pedestrian is in the vertical elevator and starts to move, and the substate switches to S 21 ;

[0044] When -0.2N≤Count k,fp ≤0.2N, indicating that at time k, the substate is S 21 Switch to S 22 ;

[0045] When -N≤Count k,fp ≤-0.9N, indicating that at time k, the substate is S 22 Switch to S 23 .

[0046] Furthermore, the zero-speed detection threshold is estimated in step (1) as follows: the center of the foot-bound inertial sensor is taken as the coordinate origin O, the right direction is the X-axis, the Y-axis is perpendicular to the X-axis and points forward, and the Z-axis points upward; the pedestrian foot-bound IMU data is collected, and the zero-speed detection threshold is obtained according to the data characteristics of the acceleration and angular velocity in the initial static interval; the detection threshold includes the minimum threshold value T of the acceleration modulus A,min , the maximum threshold value T of the acceleration modulus A,max , the threshold of the acceleration standard deviation Threshold T of angular velocity modulus W , the threshold of the standard deviation of angular velocity The calculation formula is as follows:

[0047]

[0048] Among them, A staic is the mean value of the acceleration modulus in the static interval, σ A_static is the standard deviation of the acceleration modulus in the static interval, W staic is the mean value of the angular velocity modulus in the static interval, σ W_static is the standard deviation of the angular velocity modulus in the static interval; c A,min Expressed as the minimum constant coefficient of acceleration modulus, c A,max Expressed as the maximum constant coefficient of the acceleration modulus, Expressed as the standard deviation of acceleration, Expressed as the modulus of angular velocity, c WExpressed as a constant coefficient for the standard deviation.

[0049] Furthermore, step (2) specifically includes the following steps:

[0050] (2.1) Calculate and deduct the accelerometer bias ε from the IMU static data b and the gyro bias

[0051] (2.2) The initial attitude angle is calculated using the specific force output of the static accelerometer, which mainly includes the pitch angle θ and the roll angle γ:

[0052]

[0053]

[0054] Among them, g x is the X-axis specific force output of the static foot-mounted IMU; g y is the Y-axis specific force output of the static foot-mounted IMU; g z It is the Z-axis specific force output of the static foot-mounted IMU;

[0055] (2.3) Real-time calculation of pedestrians’ posture, speed, and position

[0056] The posture update algorithm formula of the foot-mounted IMU is as follows:

[0057]

[0058] Where Q represents the quaternion matrix, t k-1 represents the k-1 moment, t k represents the k moment, i.e. the next sampling moment after k-1 moment; ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle. To express a gesture;

[0059] The speed update algorithm formula of the foot-mounted IMU is as follows:

[0060]

[0061] Among them, V k is the velocity vector of the geographic system at time k, V k-1 is the motion velocity vector in the geographic system at time k-1; Represents the coordinate transformation matrix from the aircraft system to the navigation system, Represents specific force, g n represents the local gravitational acceleration, Δt represents the sampling time interval;

[0062] The position update algorithm formula of the foot-mounted IMU is as follows:

[0063]

[0064] Among them, L represents longitude, λ represents latitude, and h represents altitude; R m Represents the radius of curvature of the Earth's meridian plane, R n It represents the radius of curvature of the Earth's angular surface.

[0065] Furthermore, the gait detection model of the four nodes of the leg and foot in step (3) is:

[0066]

[0067]

[0068]

[0069]

[0070] ZVD(k)=S1(k)&S2(k)&S3(k)&S4(k)

[0071] in, represents the mean value of acceleration from time k to time k+N, σ A,k:k+n represents the standard deviation of acceleration from time k to time k+N, represents the mean value of the angular velocity from time k to time k+N, σ W,k:k+N represents the standard deviation of the angular velocity from the kth to k+Nth time; T A,min is the minimum threshold of acceleration, T A,max is the maximum threshold of acceleration, is the threshold value of acceleration standard deviation, T W is the threshold of angular velocity, is the threshold of the angular velocity standard deviation; when S1(k), S2(k), S3(k), and S4(k) are all 1 at the same time, ZVD(k)=1, indicating that the current state is zero speed.

[0072] Furthermore, step (4) specifically includes the following steps:

[0073] (4.1) When the foot is detected to be in a zero-speed state, the Kalman filter is used to perform error estimation and construct an 18-dimensional Kalman filter state quantity X:

[0074]

[0075] in, Respectively represent the platform error angles in the east, north, and celestial directions; δV=[δV E δV N δV U ]T are the velocity errors in the east, north and celestial directions respectively; δP=[δL δλ δh] T Represent the errors of longitude, latitude and altitude respectively; Respectively represent the zero bias errors of the gyroscope X-axis, Y-axis, and Z-axis; Represent the first-order Markov drift errors of the gyroscope X-axis, Y-axis, and Z-axis respectively; They represent the first-order Markov drift errors of the accelerometer X-axis, Y-axis, and Z-axis respectively;

[0076] (4.2) Establish the state equation:

[0077]

[0078] Where A represents the state transfer matrix, G represents the system noise transfer matrix, and W represents the system noise;

[0079] (4.3) Establish the measurement equation:

[0080] Z zupt =δV=V INS -V ZUPT =HX+v

[0081] Among them, Z zupt Represents the observed value, which is the IMU velocity error δV, V INS is the IMU velocity vector obtained by strapdown inertial navigation, V ZUPT The theoretical value is [0 0 0] T , H is the observation matrix, v is the observation noise matrix, which represents the uncertainty of velocity observation;

[0082] (4.4) Navigation error correction and IMU device error correction:

[0083] According to steps (4.1), (4.2), and (4.3), a Kalman filter is constructed to obtain the optimal estimated state quantity Deduct the first nine-dimensional state quantities from the strapdown inertial navigation solution in step (2) Get the corrected attitude, velocity and position, and deduct the nine-dimensional state from the IMU data Correct the zero bias error and first-order Markov drift error of IMU.

[0084] Furthermore, in step (5), the pedestrian speed, posture and position are corrected by using a filtering method as follows:

[0085]

[0086]

[0087]

[0088]

[0089] Among them, A represents the state transfer matrix, Q represents the state transfer covariance matrix, represents the prior estimated covariance at time k, P k-1 represents the posterior estimated covariance at time k-1, K k represents the Kalman filter gain, J represents the Jacobian matrix, R represents the measurement noise covariance, Z represents the observed value, represents the prior estimate of the state at time k obtained from the system equation, X k The optimal estimate obtained by solving the Kalman filter, represents the posterior estimated covariance at time k, and I represents the identity matrix;

[0090] Solve the optimal estimated error state quantity X k Finally, the position, velocity, attitude and IMU device errors are corrected again.

[0091] Beneficial effects: Compared with the prior art, the significant advantages of the present invention are: by analyzing the motion characteristics of different motion platforms in different states, different speed constraints are applied in a targeted manner, and the speed, posture and position of pedestrians are corrected in combination with filtering methods, thereby effectively compensating for the problem of reliable inertial navigation of pedestrians in motion platform scenarios and effectively suppressing the error divergence of the inertial navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] Figure 1 Shown is a flow chart of the present invention;

[0093] Figure 2 Shown is a state classification diagram of the escalator in the present invention;

[0094] Figure 3 Shown is a sub-state classification diagram of a vertical elevator in the present invention;

[0095] Figure 4 The figure shows the comparison between the escalator positioning result of the method of this embodiment and the result of the traditional method;

[0096] Figure 5 The figure shows a comparison between the vertical elevator positioning results of the method of this embodiment and the results of the traditional method. DETAILED DESCRIPTION

[0097] The technical solution of the present invention is further described below in conjunction with the accompanying drawings.

[0098] like Figure 1 As shown, a pedestrian inertial navigation method suitable for a motion platform scene includes the following steps:

[0099] Step 1: Collect pedestrian foot-mounted IMU data and use 1000 static data output by the foot-mounted IMU to obtain the zero-speed detection threshold.

[0100] Specifically, the center of the foot-bound inertial sensor is taken as the coordinate origin O, the right direction is the X-axis, the Y-axis is perpendicular to the X-axis and points forward, and the Z-axis points upward. The three axes satisfy the right-hand screw rule. 1000 static data output by the foot-bound inertial sensor are collected, and the acceleration and angular velocity thresholds of zero-speed detection are obtained according to the data characteristics of acceleration and angular velocity in the initial static interval.

[0101] The detection threshold includes the minimum threshold T of the acceleration modulus A,min , the maximum threshold value T of the acceleration modulus A,max , the threshold of the acceleration standard deviation Threshold T of angular velocity modulus W , the threshold of the standard deviation of angular velocity The calculation formula is as follows:

[0102]

[0103] Among them, A staic is the mean value of the acceleration modulus in the static interval, σ A_static is the standard deviation of the acceleration modulus in the static interval, W staic is the mean value of the angular velocity modulus in the static interval, σ W_static is the standard deviation of the angular velocity modulus in the static interval; c A,min Expressed as the minimum constant coefficient of acceleration modulus, c A,max Expressed as the maximum constant coefficient of the acceleration modulus, Expressed as the standard deviation of acceleration, Expressed as the modulus of angular velocity, c W Expressed as a constant coefficient for the standard deviation.

[0104] Step 2: At the initial stationary moment, perform initial attitude solution and device zero bias calibration, and then use the strapdown inertial navigation method for real-time solution.

[0105] (1) Calculate and deduct the accelerometer zero bias ε from the IMU static data b and the gyro bias

[0106] (2) The initial attitude angle is calculated using the specific force output of the static accelerometer, which mainly includes the pitch angle θ and the roll angle γ:

[0107]

[0108]

[0109] Among them, g x is the X-axis specific force output of the static foot-mounted IMU; g y is the Y-axis specific force output of the static foot-mounted IMU; g z It is the Z-axis specific force output of the static foot-mounted IMU;

[0110] (3) Real-time calculation of pedestrians’ posture, speed, and position

[0111] The posture update algorithm formula of the foot-mounted IMU is as follows:

[0112]

[0113]

[0114] Where Q represents the quaternion matrix, t k-1 represents the k-1 moment, t k represents the kth moment, i.e., the next sampling moment after the k-1th moment, Δθ represents the angle increment of the three axes of the gyroscope during the sampling time, ΔΘ represents the antisymmetric matrix composed of Δθ, ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle. To express a gesture;

[0115] The speed update algorithm formula of the foot-mounted IMU is as follows:

[0116]

[0117] Among them, V k is the velocity vector of the geographic system at time k, V k-1 is the motion velocity vector in the geographic system at time k-1; Represents the coordinate transformation matrix from the aircraft system to the navigation system, Represents specific force, g n represents the local gravitational acceleration, Δt represents the sampling time interval;

[0118] The position update algorithm formula of the foot-mounted IMU is as follows:

[0119]

[0120] Among them, L represents longitude, λ represents latitude, and h represents altitude; R m Represents the radius of curvature of the Earth's meridian plane, R n V represents the radius of curvature of the Earth's umbo plane; E represents the velocity vector in the east direction, V N represents the velocity vector in the north direction, V U Represents the velocity vector in the celestial direction.

[0121] Step 3: Establish a gait detection model, use the acceleration modulus, foot acceleration standard deviation, angular velocity modulus, and angular velocity standard deviation as detection conditions, and combine the estimated zero-speed detection threshold to perform zero-speed detection. The foot movement state is divided into the swing phase and the support phase. When the detection is in the zero-speed state, the foot is in the support phase, otherwise it is in the swing phase.

[0122] Specifically, the gait detection model of the four nodes of the legs and feet is:

[0123]

[0124]

[0125]

[0126]

[0127] ZVD(k)=S1(k)&S2(k)&S3(k)&S4(k)

[0128] in, represents the mean value of acceleration from time k to time k+N, σ A,k:k+n represents the standard deviation of acceleration from time k to time k+N, represents the mean value of the angular velocity from time k to time k+N, σ W,k:k+N represents the standard deviation of the angular velocity from the kth to k+Nth time; T A,min is the minimum threshold of acceleration, T A,max is the maximum threshold of acceleration, is the threshold value of acceleration standard deviation, T W is the threshold of angular velocity, is the threshold of the angular velocity standard deviation; when labels S1(k), S2(k), S3(k), and S4(k) are all 1, ZVD(k)=1, indicating that the current state is zero speed.

[0129] Step 4: During the strapdown inertial navigation system solution process, when it is detected that the foot is in a zero-speed state, the Kalman filter calculation is used to estimate the error and obtain the corrected position and speed, otherwise no correction is performed;

[0130] (4.1) When the foot is detected to be in a zero-speed state, the Kalman filter is used to perform error estimation and construct an 18-dimensional Kalman filter state quantity X:

[0131]

[0132] in, Respectively represent the platform error angles in the east, north, and celestial directions; δV=[δV E δV NδV U ] T are the velocity errors in the east, north and celestial directions respectively; δP=[δL δλ δh] T Represent the errors of longitude, latitude and altitude respectively; Respectively represent the zero bias errors of the gyroscope X-axis, Y-axis, and Z-axis; Represent the first-order Markov drift errors of the gyroscope X-axis, Y-axis, and Z-axis respectively; They represent the first-order Markov drift errors of the accelerometer X-axis, Y-axis, and Z-axis respectively;

[0133] (4.2) Establish the state equation:

[0134]

[0135] Where A represents the state transfer matrix, G represents the system noise transfer matrix, and W represents the system noise; is the first-order derivative of the state quantity X; W = [w gx w gy w gz ] T , w gx is the white noise of the gyroscope X axis, w gy is the white noise of the gyro Y axis, w gz is the white noise of the gyroscope Z axis;

[0136] (4.3) Establish the measurement equation:

[0137] Z zupt =δV=V INS -V ZUPT =HX+v

[0138] Among them, Z zupt Represents the observed value, which is the IMU velocity error δV, V INS is the IMU velocity vector obtained by strapdown inertial navigation, V ZUPT The theoretical value is [0 0 0] T , H is the observation matrix, v is the observation noise matrix, which represents the uncertainty of velocity observation;

[0139] (4.4) Navigation error correction and IMU device error correction:

[0140] According to steps (4.1), (4.2), and (4.3), a Kalman filter is constructed to obtain the optimal estimated state quantity That is, the estimated error of attitude, speed and position; deduct the first nine-dimensional state quantity from the strapdown inertial navigation solution result in step 2 Get the corrected attitude, velocity and position, and deduct the nine-dimensional state from the IMU data Correct the zero bias error and first-order Markov drift error of IMU.

[0141] Step 5: Compare the relevant speed calculated in step 2 with the set threshold to determine whether there is a motion platform at the current moment and the type of motion platform; when a certain type of motion platform is detected, classify the motion sub-states of the platform, impose different speed constraints according to the motion characteristics of different sub-states, and use filtering methods to correct the pedestrian speed, posture and position; if no motion platform is detected, repeat steps 2 to 3. The details are as follows:

[0142] (1) Motion platform detection and classification:

[0143] Step 2: The velocity vector V in the geographic system obtained by calculation k and the celestial velocity vector The modulus value is used for motion platform detection and classification:

[0144] The escalator detection formula is as follows:

[0145]

[0146] Among them, T escal,norm represents the speed threshold of the escalator, T escal,U Indicates the escalator's celestial speed threshold;

[0147] The vertical elevator detection formula is as follows:

[0148]

[0149] It means that at time k, the pedestrian is in the vertical elevator and the elevator starts running; T ele,norm represents the speed threshold of the vertical elevator, T ele,U Indicates the celestial velocity threshold of the vertical elevator.

[0150] (2) The presence of a certain type of motion platform is detected, and the sub-state classification of the motion of the platform is performed simultaneously. The sub-state classification model is:

[0151] (2.1) Taking the above behavior as an example, according to the different directions of the step velocity vectors during the operation of the escalator, the entire motion state of the escalator is divided into five sub-states S11-S15, and the five sub-states are switched in a fixed order (S11-S12-S13-S14-S15), as follows Figure 2 As shown:

[0152] Substate S11, indicating that the pedestrian is at the entrance of the escalator; the elevator has only forward velocity, and the velocity vector is constant;

[0153] Substate S12 indicates that the pedestrian is at the turning position near the entrance of the escalator; the forward speed of the elevator decreases, the vertical speed increases, and the speed modulus is constant, which is consistent with substate S11;

[0154] Substate S13 indicates that the pedestrian is on the escalator and moves upward with the escalator; the escalator has a forward velocity and a vertical velocity, the velocity vector is constant, and the velocity modulus is consistent with substate S11;

[0155] Substate S14 indicates that the pedestrian is at a turning position close to the escalator exit; the forward speed of the elevator increases, the vertical speed decreases, and the speed modulus is consistent with substate S11;

[0156] In substate S15, it indicates that the pedestrian is at the exit of the escalator, the escalator has only forward velocity, the velocity vector is constant, and the velocity modulus is consistent with substate S11.

[0157] (2.2) According to the different changing rules of the velocity vector modulus during the operation of the vertical elevator, the full motion state of the vertical elevator is divided into three sub-states S21-S23, and the three sub-states are switched in a fixed order (S21-S22-S23) as follows Figure 3 At the same time, during the operation of the vertical elevator, there is only vertical speed, and the horizontal speed is 0;

[0158] Substate S21 indicates that the vertical elevator accelerates upward, and the vertical speed gradually increases from 0 to a constant value;

[0159] Substate S22 indicates that the vertical elevator is running upward at a constant speed, and the vertical speed is constant;

[0160] Substate S23 indicates that the vertical elevator decelerates and moves upward, and the vertical speed decreases to 0.

[0161] (3) From the above analysis, we can see that by comparing the positive and negative absolute acceleration in the vertical direction within the sliding window, we can get the forward search label Count k,fp Determine whether the motion sub-state switching condition is met. Specific sub-state classification algorithms for different platforms:

[0162]

[0163] Among them, Count k,fp represents the forward search label, N represents the sliding window length, sgn() represents the sign function, and returns 1 if the parameter is a positive number, returns 0 if the parameter is 0, and returns nothing if the parameter is a negative number; A U Indicates the acceleration azimuth specific force output, g is the local gravitational acceleration value.

[0164] When the moving platform is an escalator, take the upward movement of the escalator as an example:

[0165] When Count k,fp =1, indicating that at time k, the substate switches to S 11 ;

[0166] When 0.9N≤Count k,fp ≤N, indicating that at time k, the substate is S 11 Switch to S 12 ;

[0167] When -0.2N≤Count k,fp ≤0.2N, indicating that at time k, the substate is S 12 Switch to S 13 ;

[0168] When -N≤Count k,fp ≤-0.9N, indicating that at time k, the substate is S 13 Switch to S 14 ;

[0169] When -0.2N≤Count k,fp ≤0.2N, indicating that at time k, the substate is S 14 Switch to S 15 ;

[0170] When the motion platform is a vertical elevator, take the vertical elevator ascent as an example:

[0171] When 0.9N≤Count k,fp ≤N, indicating that at time k, the pedestrian is in the vertical elevator and starts to move, and the substate switches to S 21 ;

[0172] When -0.2N≤Count k,fp ≤0.2N, indicating that at time k, the substate is S 21 Switch to S 22 ;

[0173] When -N≤Count k,fp ≤-0.9N, indicating that at time k, the substate is S 22 Switch to S 23 .

[0174] In summary, the time intervals of each motion sub-state of the vertical elevator and escalator can be determined.

[0175] (4) Set corresponding speed constraints based on sub-state classification:

[0176] According to the motion characteristics of different sub-states during the motion of escalators and vertical elevators, different speed constraint methods are set, as shown in the following table:

[0177]

[0178] Among them, the lateral zero speed correction, horizontal zero speed correction, and vertical zero speed correction methods are all low-dimensional representations of the zero speed correction method in step 4. The specific formulas are:

[0179] Z zupt,lateral =δV lateral =V E -0=H lateral X+v lateral .

[0180] Z zupt,horiz =δV horiz =[V E V N ] T -[0 0] T =H horiz X+v horiz

[0181] Z zupt,vert =δV vert =V U -0=H vert X+v vert

[0182] Among them, Z zupt,lateral represents the observed value of lateral zero-speed correction, Z zupt_horiz Represents the observed value of horizontal zero-speed correction, Z zupt,vert Represents the observed value of vertical zero-speed correction; δV lateral Indicates the IMU velocity error of the lateral zero-speed correction, δV horiz Indicates the IMU velocity error of horizontal zero speed correction, δV vert Indicates the IMU velocity error of vertical velocity correction; V E represents the velocity vector in the east direction, V N represents the velocity vector in the north direction, V U H represents the velocity vector in the celestial direction; lateral =[0 1×3 1 0 1×14 ], H horiz =[0 2×3 I 2×2 0 2×13 ], H vert =[0 1×5 1 0 1×12 ];v lateral represents the observation noise matrix of lateral zero-speed correction, vhoriz represents the observation noise matrix of lateral zero-speed correction, v vert represents the observation noise matrix of the vertical zero velocity correction.

[0183] The speed modulus constant correction method formula is as follows:

[0184]

[0185]

[0186] Z cons_norm =δV k,norm =V k,norm -V ob,norm =f(V k )-V ob,norm +n n

[0187] Among them, T 11 and T 12 Indicates entering substate S 11 and S 12 Time; V ob,norm Represents the motion substate S 11 The mean value of the velocity vector modulus obtained during the process, V k,norm represents the modulus of the velocity vector at time k, represents the eastward velocity vector at time k, represents the velocity vector to the north at time k, represents the velocity vector in the celestial direction at time k; f( ) represents the modulus function, Z cons_norm represents the observed value of the constant correction of the velocity modulus, δV k,norm represents the velocity modulus error of the foot-bound IMU at time k, n n is the observation noise matrix;

[0188] After the nonlinear equation above is expanded by Taylor series and the linear terms are retained, the linear error measurement equation is obtained:

[0189] Z cons_n =δV k,norm =JX+n n

[0190] Where J can represent the Jacobian matrix of the nonlinear function relative to the system state vector X, and the formula is

[0191]

[0192] The observation equation formula of the forward and vertical velocity vector constant correction method is as follows:

[0193]

[0194] Among them, Z cons_vect The velocity error observation that represents the constant correction of the forward and vertical velocity vector; δV k,vect represents the difference between the east and north velocity vectors at time k and time k-1, n v is the observation noise matrix; H cons_vect =[0 2×3 I 2×2 0 2×13 ].

[0195] According to the above system equations and measurement equations, the extended Kalman filter method is applied to correct the pedestrian speed, posture and position as follows;

[0196]

[0197]

[0198]

[0199]

[0200] Among them, A represents the state transfer matrix, Q represents the state transfer covariance matrix, represents the prior estimated covariance at time k, P k-1 represents the posterior estimated covariance at time k-1, K k represents the Kalman filter gain, J represents the Jacobian matrix, R represents the measurement noise covariance, Z represents the observed value, represents the prior estimate of the state at time k obtained from the system equation, X k The optimal estimate obtained by solving the Kalman filter, represents the posterior estimated covariance at time k, and I represents the identity matrix;

[0201] Solve the optimal estimated error state quantity X k Finally, the position, velocity, attitude and IMU device errors are corrected again.

[0202] The experimental scene of this embodiment uses a teaching laboratory building, which includes escalator and vertical elevator experiments, with total route lengths of 41.5m and 63.2m respectively.

[0203] like Figure 4As shown, in the escalator experiment, the tester walked forward at a speed of 1.5m / s, and did not stop when encountering an escalator during the journey, and rode normally. In the process of riding the elevator, the navigation solution result using the zero-speed correction algorithm only when walking is shown in the dotted line. The inertial guidance error diverges rapidly, and the start and end point errors are greater than 20 meters; the solution result using the present invention, i.e., a pedestrian inertial navigation method suitable for a motion platform scenario, is shown in the solid line. The start and end position error is 0.53 meters, and the horizontal and height positioning errors are greatly reduced, effectively improving the positioning accuracy.

[0204] like Figure 5 As shown in the figure, in the vertical elevator experiment, in a building with a total height of 10 floors, the tester was stationary in the elevator on the first floor at the beginning of the experiment, and then stopped 8 times at random floors, and finally stopped at the second floor. The result calculated by the navigation method without using the moving platform speed error is shown as the dotted line in the figure. The inertial guidance error diverges rapidly, and the start and end errors are greater than 100 meters; the result calculated by using the pedestrian inertial navigation method of the present invention, which is suitable for the moving platform scene, is shown as the solid line in the figure. The start and end position errors are 1.7 meters, and the height positioning errors are greatly reduced, which effectively improves the positioning accuracy.

Claims

1. A pedestrian inertial navigation method suitable for a motion platform scene, characterized in that: The following steps are involved: (1) Collect pedestrian foot-mounted IMU data and estimate the zero-speed detection threshold based on the initial static data; (2) Preprocess the collected foot-worn IMU data and use the strapdown inertial navigation method to perform real-time calculation of speed, attitude, and position; (3) Establish a four-node gait detection model for the legs and feet, and use the estimated zero-speed detection threshold to perform zero-speed detection; (4) When the zero-speed detection result is in the zero-speed state, the Kalman filter is used to perform zero-speed correction to obtain the corrected speed, attitude and position, and to perform error correction on the IMU device; Otherwise, no correction is made; (5) Compare the relevant speed calculated according to step (2) with the set threshold to determine whether there is a motion platform at the current moment and the type of the motion platform; when a certain type of motion platform is detected, classify the motion sub-states of the platform, impose different speed constraints according to the motion characteristics of different sub-states, and use filtering methods to correct the pedestrian speed, posture and position; if no motion platform is detected, repeat steps (2)-(5); The motion platform detection and classification described in step (5) are specifically as follows: If satisfied: It means that the pedestrian is on the escalator at time k; Among them, V k represents the velocity vector in the geographic system, represents the celestial velocity vector, T escal,norm represents the speed threshold of the escalator, T escal,U Indicates the escalator's celestial speed threshold; If satisfied: It means that at time k, the pedestrian is in the vertical elevator and the elevator starts running; where V k represents the velocity vector in the geographic system, represents the celestial velocity vector, T ele,norm represents the speed threshold of the vertical elevator, T ele,U Indicates the celestial velocity threshold of the vertical elevator; When the escalator is in the current moment, the whole motion state of the escalator is divided into five sub-states S 11 —S 15 , and the five sub-states are switched in a fixed order; among them, sub-state S 11 Indicates that the pedestrian is at the entrance of the escalator. The speed constraint method is: first perform lateral zero speed correction and then perform vertical zero speed correction; sub-state S 12 Indicates that the pedestrian is at the turning position near the escalator entrance. The speed constraint method is: first perform lateral zero speed correction and then perform speed modulus constant correction; sub-state S 13 Indicates that the pedestrian is on the escalator, and the speed constraint method is: first perform lateral zero speed correction and then perform forward and vertical constant speed correction; sub-state S 14 Indicates that the vehicle is at a turning position near the escalator exit. The speed constraint method is: first perform lateral zero speed correction and then perform speed modulus constant correction. Substate S 15 Indicates that the pedestrian is at the escalator exit, and the speed constraint method is: first perform lateral zero speed correction and then perform vertical zero speed correction; When the current moment is in the vertical elevator, the whole motion state of the vertical elevator is divided into three sub-states S 21 —S 23 , and the three sub-states switch in a fixed order; sub-state S 21 Indicates that the vertical elevator accelerates upward. The speed constraint method is: perform horizontal zero speed correction; sub-state S 22 Indicates that the vertical elevator is running upward at a constant speed. The speed constraint method is: first perform horizontal zero speed correction and then perform vertical constant speed correction; sub-state S 23 Indicates that the vertical elevator decelerates and moves upward. The speed constraint method is: perform horizontal zero speed correction; Specifically, the classification algorithm formula of the sub-state is: Among them, Count k,fp represents the forward search label, N represents the sliding window length, sgn() represents the symbol function, A U It indicates the acceleration azimuth force output, where g is the local gravitational acceleration value.

2. The pedestrian inertial navigation method applicable to a motion platform scene according to claim 1, characterized in that: The lateral zero-speed correction, horizontal zero-speed correction, and vertical zero-speed correction methods are all low-dimensional representations of the zero-speed correction method. The specific formulas are: Z zupt,lateral =δV lateral =V E -0=H lateral X+v lateral 。 Z zupt,horiz =δV horiz =[V E V N ] T -[00] T =H horiz X+v horiz Z zupt,vert =δV vert =V U -0=H vert X+v vert Among them, Z zupt,lateral represents the observed value of lateral zero-speed correction, Z zupt_horiz Represents the observed value of horizontal zero-speed correction, Z zupt,vert Represents the observed value of vertical zero-speed correction; δV lateral Indicates the IMU velocity error of the lateral zero-speed correction, δV horiz Indicates the IMU velocity error of horizontal zero speed correction, δV vert Indicates the IMU velocity error of vertical velocity correction; V E represents the velocity vector in the east direction, V N represents the velocity vector in the north direction, V U H represents the velocity vector in the celestial direction; lateral =[0 1×3 10 1×14 ], H horiz =[0 2×3 I 2×2 0 2×13 ], H vert =[0 1×5 10 1×12 ];v lateral represents the observation noise matrix of lateral zero-speed correction, v horiz represents the observation noise matrix of lateral zero-speed correction, v vert represents the observation noise matrix of vertical zero-speed correction; The speed modulus constant correction method formula is as follows: Z cons_norm =δV k,norm =V k,norm -V ob,norm =f(V k )-V ob,norm +n n Among them, T 11 and T 12 Indicates entering substate S 11 and S 12 Time; V ob,norm Represents the motion substate S 11 The mean value of the velocity vector modulus obtained during the process, V k,norm represents the modulus of the velocity vector at time k, represents the eastward velocity vector at time k, represents the velocity vector to the north at time k, represents the velocity vector in the celestial direction at time k; f() represents the modulus function, Z cons_norm represents the observed value of the constant correction of the velocity modulus, δV k,norm represents the velocity modulus error of the foot-bound IMU at time k, n n is the observation noise matrix; The observation equation formula of the forward and vertical velocity vector constant correction method is as follows: Among them, Z cons_vect The velocity error observation that represents the constant correction of the forward and vertical velocity vector; δV k,vect represents the difference between the east and north velocity vectors at time k and time k-1, n v is the observation noise matrix; H cons_vect =[0 2×3 I 2×2 0 2×13 ]。 3. The pedestrian inertial navigation method applicable to a motion platform scene according to claim 1, characterized in that: Specifically, the classification of the sub-states is as follows: When the moving platform is an escalator, When Count k,fp =1, indicating that at time k, the substate switches to S 11 ; When 0.9N≤Count k,fp ≤N, indicating that at time k, the substate is S 11 Switch to S 12 ; When -0.2N≤Count k,fp ≤0.2N, indicating that at time k, the substate is S 12 Switch to S 13 ; When -N≤Count k,fp ≤-0.9N, indicating that at time k, the substate is S 13 Switch to S 14 ; When -0.2N≤Count k,fp ≤0.2N, indicating that at time k, the substate is S 14 Switch to S 15 ; When the motion platform is a vertical elevator, When 0.9N≤Count k,fp ≤N, indicating that at time k, the pedestrian is in the vertical elevator and starts to move, and the substate switches to S 21 ; When -0.2N≤Count k,fp ≤0.2N, indicating that at time k, the substate is S 21 Switch to S 22 ; When -N≤Count k,fp ≤-0.9N, indicating that at time k, the substate is S 22 Switch to S 23 .

4. The pedestrian inertial navigation method applicable to a motion platform scene according to claim 1, characterized in that: The specific method for estimating the zero-speed detection threshold in step (1) is as follows: the center of the foot-bound inertial sensor is taken as the coordinate origin O, the right direction is the X-axis, the Y-axis is perpendicular to the X-axis and points forward, and the Z-axis points upward; the pedestrian's foot-bound IMU data is collected, and the zero-speed detection threshold is obtained according to the data characteristics of the acceleration and angular velocity in the initial static interval; the detection threshold includes the minimum threshold value T of the acceleration modulus value A,min , the maximum threshold value T of the acceleration modulus A,max , the threshold of the acceleration standard deviation Threshold T of angular velocity modulus W , the threshold of the standard deviation of angular velocity The calculation formula is as follows: Among them, A staic is the mean value of the acceleration modulus in the static interval, σ A_static is the standard deviation of the acceleration modulus in the static interval, W staic is the mean value of the angular velocity modulus in the static interval, σ W_static is the standard deviation of the angular velocity modulus in the static interval; c A,min Expressed as the minimum constant coefficient of the acceleration modulus, c A,max Expressed as the maximum constant coefficient of the acceleration modulus, Expressed as the standard deviation of acceleration, Expressed as the modulus of angular velocity, c W Expressed as a constant coefficient for the standard deviation.

5. The pedestrian inertial navigation method applicable to a motion platform scene according to claim 1, characterized in that: Step (2) specifically includes the following steps: (2.1) Calculate and deduct the accelerometer bias ε from the IMU static data b and the gyro bias (2.2) The initial attitude angle is calculated using the specific force output of the static accelerometer, including the pitch angle θ and the roll angle γ: Among them, g x is the X-axis specific force output of the static foot-mounted IMU; g y is the Y-axis specific force output of the static foot-mounted IMU; g z It is the Z-axis specific force output of the static foot-mounted IMU; (2.3) Real-time calculation of pedestrians’ posture, speed, and position The posture update algorithm formula of the foot-mounted IMU is as follows: Where Q represents the quaternion matrix, t k-1 represents the k-1 moment, t k represents the k moment, i.e. the next sampling moment after k-1 moment; ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle. To express a gesture; The speed update algorithm formula of the foot-mounted IMU is as follows: Among them, V k is the velocity vector of the geographic system at time k, V k-1 is the motion velocity vector in the geographic system at time k-1; Represents the coordinate transformation matrix from the aircraft system to the navigation system, Represents specific force, g n represents the local gravitational acceleration, Δt represents the sampling time interval; The position update algorithm formula of the foot-mounted IMU is as follows: Among them, L represents longitude, λ represents latitude, and h represents altitude; R m Represents the radius of curvature of the Earth's meridian plane, R n It represents the radius of curvature of the earth's angular surface.

6. The pedestrian inertial navigation method applicable to a motion platform scene according to claim 1, characterized in that: The gait detection model of the four nodes of the leg and foot in step (3) is: ZVD(k)=S1(k)&S2(k)&S3(k)&S4(k) in, represents the mean value of acceleration from time k to time k+N, σ A,k:k+n represents the standard deviation of acceleration from time k to time k+N, represents the mean value of the angular velocity from time k to time k+N, σ W,k:k+N represents the standard deviation of the angular velocity from the kth to k+Nth time; T A,min is the minimum threshold of acceleration, T A,max is the maximum threshold of acceleration, T σA is the threshold value of the acceleration standard deviation, T W is the threshold value of angular velocity, T σW is the threshold of the angular velocity standard deviation; when S1(k), S2(k), S3(k), and S4(k) are all 1 at the same time, ZVD(k)=1, indicating that the current state is zero speed.

7. The pedestrian inertial navigation method applicable to a motion platform scene according to claim 1, characterized in that: Step (4) specifically includes the following steps: (4.1) When the foot is detected to be in a zero-speed state, the Kalman filter is used to perform error estimation and construct an 18-dimensional Kalman filter state quantity X: in, Respectively represent the platform error angles in the east, north, and celestial directions; δV=[δV E δV N δV U ] T are the velocity errors in the east, north and celestial directions respectively; δP=[δLδλδh] T Represent the errors of longitude, latitude and altitude respectively; Respectively represent the zero bias errors of the gyroscope X-axis, Y-axis, and Z-axis; Represent the first-order Markov drift errors of the gyroscope X-axis, Y-axis, and Z-axis respectively; They represent the first-order Markov drift errors of the accelerometer X-axis, Y-axis, and Z-axis respectively; (4.2) Establish the state equation: Where A represents the state transfer matrix, G represents the system noise transfer matrix, and W represents the system noise; (4.3) Establish the measurement equation: Z zupt =δV=V INS -V ZUPT =HX+v Among them, Z zupt Represents the observed value, which is the IMU velocity error δV, V INS is the IMU velocity vector obtained by strapdown inertial navigation, V ZUPT The theoretical value is [0 0 0] T , H is the observation matrix, v is the observation noise matrix, which represents the uncertainty of velocity observation; (4.4) Navigation error correction and IMU device error correction: According to steps (4.1), (4.2), and (4.3), a Kalman filter is constructed to obtain the optimal estimated state quantity Deduct the first nine-dimensional state quantities from the strapdown inertial navigation solution in step (2) Get the corrected attitude, velocity and position, and deduct the nine-dimensional state from the IMU data Correct the zero bias error and first-order Markov drift error of IMU.

8. The pedestrian inertial navigation method applicable to a motion platform scene according to claim 1, characterized in that: In step (5), the filtering method is used to correct the pedestrian speed, posture and position as follows: Among them, A represents the state transfer matrix, Q represents the state transfer covariance matrix, represents the prior estimated covariance at time k, P k-1 represents the posterior estimated covariance at time k-1, K k represents the Kalman filter gain, J represents the Jacobian matrix, R represents the measurement noise covariance, Z represents the observed value, represents the prior estimate of the state at time k obtained from the system equation, X k The optimal estimate obtained by solving the Kalman filter, represents the posterior estimated covariance at time k, and I represents the identity matrix; Solve the optimal estimated error state quantity X k Finally, the position, velocity, attitude and IMU device errors are corrected again.

Images