Pedestrian integrated navigation method based on multi-error factor tight coupling model

Abstract

The present disclosure provides a pedestrian integrated navigation method based on a multi-error factor tight coupling model, which uses a sensor arranged on the chest to collect inertial measurement data; an INS performs inertial navigation calculation based on the inertial measurement data, and updates the INS position; an iterative principal component analysis method is used to determine the angle error between the heading and the walking direction under multi-gait conditions, to compensate for the MetCM heading based on quaternion calculation, and to obtain the updated and compensated MetCM position; a Kalman filter combining the MetCM model and the INS error model is constructed to estimate the position error, to compensate for the INS position and the MetCM position, and to output the calibrated position. The present disclosure can effectively improve the positioning accuracy of autonomous navigation.

Description

Technical Field

[0001] This invention relates to the fields of navigation, guidance and control, and specifically to a pedestrian integrated navigation method based on a tightly coupled model with multiple error factors. Background Technology

[0002] Upper limb strapdown pedestrian dead reckoning (PDR) is a pedestrian navigation and positioning method with strong autonomy, low drift, and excellent wearability, widely used in various fields such as individual combat, disaster relief, and daily life. This method relies on the Inertial Measurement Unit (IMU) to self-estimate the single-step step length and heading angle, combining them to form the single-step displacement increment and thus updating the pedestrian's position. Specifically, the IMU is strapped to the upper limb of the pedestrian, such as the waist or chest. The Inertial Navigation System (INS) continuously estimates the heading angle, assuming the pedestrian's direction of movement is consistent with the heading. Since the inertial measurement data period and the single-step period are consistent, peak detection can be used to divide the single-step interval, and the pedestrian's single-step step length can be estimated based on the inertial measurement and statistical step length model within the interval. Combining the step length and heading allows for the calculation of the pedestrian's single-step displacement increment, thereby updating the pedestrian's position information.

[0003] However, existing PDR methods suffer from problems such as inaccurate estimation results of statistical step size models and offset between heading and pedestrian movement orientation under complex motion patterns. Summary of the Invention

[0004] In view of this, the present invention provides a pedestrian integrated navigation method based on a tightly coupled model of multiple error factors, which can effectively improve the positioning accuracy of autonomous navigation.

[0005] To solve the above-mentioned technical problems, the present invention is implemented as follows.

[0006] A pedestrian integrated navigation method based on a tightly coupled model with multiple error factors includes:

[0007] Step 1: Strapdown IMU to chest to collect inertial measurement data;

[0008] Step 2: The INS performs inertial navigation calculations based on the inertial measurement data and updates the INS position at time k. Speed ​​and quaternions;

[0009] Step 3: Determine the covariance matrix J of the inertial measurement data at time k. k Solve J k The eigenvector corresponding to the largest eigenvalue Using feature vectors The angular relationship with the forward axis of the carrier coordinate system is used to determine the angular error between the heading and the travel direction under multi-gait conditions.

[0010] The single-step detector detects the step length based on inertial measurement data. When the pedestrian's single-step movement ends, the quaternion is converted into a heading angle. And update the position of the MetCM based on the multi-error-factor tightly coupled model at time k. And the angular error between the heading and the direction of travel is used. Provide compensation;

[0011] Step 4: MetCM Modeling: Transform the position estimate into a position error estimate, and calculate the MetCM position error δp. n The model is coupled with the step size scaling factor error δk and the heading angle error δψ, and the step size scaling factor error δk and the heading angle error δψ are modeled.

[0012] Step 5: Design a Kalman filter by combining the MetCM model and the INS error model, estimate the position error, compensate to the INS position and MetCM position, and output the calibrated position.

[0013] Preferably, in step 3, the angular error between the heading and the walking direction under multi-step conditions is determined. The method is as follows:

[0014] Based on acceleration data from inertial measurement data, principal component analysis is used to construct the covariance matrix J of the horizontal acceleration data sequence at time k. k J is solved using an iterative algorithm. k The eigenvector corresponding to the largest eigenvalue

[0015] Kalman filtering suppression To eliminate noise, obtain the eigenvector estimate η. k ;

[0016] The IMU's sensing axes are kept aligned with the carrier coordinate system. The pedestrian's orientation on the horizontal plane of the carrier coordinate system is defined as the reference direction η. ref ; using η ref With η k η is obtained by finding the inner product relationship between them. k The angular relationship with the forward axis of the carrier coordinate system is the angular error between the heading and the direction of travel.

[0017] Preferably, the iterative solution algorithm is used to solve J. k The eigenvector corresponding to the largest eigenvalue for:

[0018] Assume there is a horizontal acceleration data sequence from time k-Δ to time k at time k [f] k f k-1 ...f k-Δ The covariance matrix J at time k is obtained using classical principal component analysis. k The calculation formula is as follows:

[0019]

[0020] In the formula, Δ represents the single-step period, and k-Δ represents the end time of the previous single-step period;

[0021] Expand J k And order The covariance matrix J k Transform into an iterative computation mode:

[0022]

[0023] μ k =μ k-1 +Δ -1 (f k -f k-Δ+1 )

[0024]

[0025] Iterative solution of J k Then calculate J k The eigenvector corresponding to the largest eigenvalue

[0026] Preferably, the Kalman filter is used to suppress... The noise reduction step employs a Kalman filter with a constant forgetting factor α.

[0027] Preferably, a Kalman filter with a constant forgetting factor α is used to construct η. k The estimation equation is:

[0028] K k =P k-1 (P k-1 +R k ) -1

[0029]

[0030] P k =α(I2-K) k )P k-1

[0031] Among them, P kLet R be the Kalman filter covariance matrix at time k. k To observe the noise covariance matrix, K k I is the filter gain, and I2 is the second-order identity matrix.

[0032] Preferably, in step 3, the angular error between the heading and the direction of travel is used. When performing compensation, the location of MetCM The update and compensation formulas are as follows:

[0033]

[0034] in, The position of the MetCM at time k-Δ. The heading angle calculated using quaternions at time k-Δ. Let be the angular error between the heading and the direction of travel at time k-Δ. This represents the single-step step size measurement at time k-Δ; Δ represents the single-step period, and k-Δ represents the end time of the previous single-step period.

[0035] Preferably, step 3 specifically includes:

[0036] Step 31: Calculate the magnitude of the acceleration data in the inertial measurement data and perform low-pass filtering on the magnitude sequence;

[0037] Step 32: Divide the pedestrian's single-step interval according to the peak detection method or the zero-crossing axis method;

[0038] Step 33: Search for the maximum value f of the acceleration modulus within a single step interval. max and minimum value f min ;

[0039] Step 34: Calculate the pedestrian's single step length

[0040] Step 35: When the pedestrian's single-step movement ends, convert the quaternion of INS into a heading angle. The MetCM position at time k is updated using the aforementioned update and compensation formula.

[0041] Preferably, in step 4, the constructed MetCM model is:

[0042] The MetCM model includes the MetCM location error δp. n Three models: step size scaling factor error δk and heading angle error δψ; among them,

[0043] The MetCM position error δp n The model is as follows:

[0044] MetCM position error δp n Substituting the step size scaling factor error δk and the heading angle error δψ into the MetCM position update formula, we obtain a nonlinear equation for the error term. We then linearize this equation to obtain a linearized equation.

[0045] Based on the fact that the step size scaling factor operates near zero and the magnetometer alignment error is within a small angular range, the step size scaling factor error δk at time k is assumed to be... k and heading angle error δψ k As the linearization equation approaches zero, the position error δp of the MetCM is simplified to obtain the position error δp. n The coupling equations with the step size scaling factor error δk and the heading angle error δψ are as follows:

[0046]

[0047] in, Let δk be the position error of the MetCM at time k-Δ. k-Δ Let δψ be the step size scaling factor error at time k-Δ. k-Δ The heading angle error at time k-Δ; This represents the single-step measurement value at time k-Δ. The heading angle at time k-Δ is calculated and compensated based on quaternions;

[0048] The modeling of the step size scaling factor error δk and the heading angle error δψ is as follows:

[0049] 1) When the system is in a state of heading inaccuracy or the step size model is not calibrated, δψ k and δk k Modeled as random constants:

[0050] δk k =δk k-Δ

[0051] δψ k =δψ k-Δ

[0052] 2) When condition 1) is satisfied, the heading angle error and step size scaling factor error exhibit randomness, and δψ k and δk k Modeled as a random walk process:

[0053]

[0054] Among them, w δk,k-Δ and w δψ,k-Δ These are noises that satisfy a normal distribution. and These are the variances of the defined normal distribution.

[0055] Preferably, step 5 specifically includes:

[0056] ① Select the INS misalignment angle φ and the INS velocity error δv n INS position error MetCM position error δp n The error state of the MetCM / INS integrated navigation system is constructed using the step size scale factor error δk.

[0057]

[0058] ②Based on error state x M / I Construct the state equations of the integrated navigation system:

[0059] x M / I,k =Φ M / I,k|k-1 x M / I,k-1

[0060]

[0061] In the formula, I i Let O be an i-order identity matrix. i*j Let f be a zero matrix with row i and column j. n × is a third-order antisymmetric matrix composed of acceleration vectors in the northeast-northeast coordinate system; x M / I,k Let Φ be the state at time k, Δt be the sampling period, and Φ be the value of Φ. M / I,k|k-1 Here is the state transition matrix. This is a single-step measurement value. The heading angle is obtained based on quaternion calculation and has been compensated.

[0062] Instagram location and MetCM location The difference is used as the observation vector Then, construct the observation equations for the integrated navigation system:

[0063] y M / I,k =[O 2*6 I2 O 2*1 -I2 O 2*1 ]x M / I,k

[0064] ③ Based on the state equation and observation equation, perform Kalman filter measurement updates to obtain the state estimation result, in which the MetCM position error δp is included. n and INS position error Similarly, the MetCM position error δp n Or INS position error Compensate to the INS and MetCM positions, and output the calibrated position.

[0065] Beneficial effects:

[0066] (1) This invention employs a combined navigation model of MetCM and INS. Compared to MetCM, which is based on a statistical step size model, INS provides richer motion information representation over a short period and relies on kinematic differential equations to estimate the attitude, velocity, and position of the pedestrian navigation system, thus achieving higher single-step size estimation accuracy under conditions of sufficient convergence. However, a drawback of INS is that it may diverge rapidly if left uncorrected for an extended period. In terms of integral order, MetCM has a lower integral order than INS and exhibits better stability over longer time scales. Therefore, this invention combines MetCM and INS to improve positioning accuracy.

[0067] (2) Considering that when the direction of pedestrian movement is inconsistent with the heading under mixed conditions such as side walking and backward walking, directly updating the position is likely to introduce a large error. Therefore, this invention compensates for the heading angle to further improve the positioning accuracy.

[0068] (3) In a preferred embodiment, the iterative principal component analysis method transforms the classical principal component analysis method into an iterative form and uses a Kalman filter to reduce noise. It is easy to see from the derivation results that its filtering result is equivalent to taking the average of multiple eigenvector results. Therefore, the iterative principal component analysis method has higher accuracy than the classical principal component analysis method. Moreover, the iterative calculation method significantly reduces the amount of computation at the update time. Attached Figure Description

[0069] Figure 1 This is a flowchart of the existing PDR method.

[0070] Figure 2 This is a flowchart of the pedestrian integrated navigation method based on a tightly coupled model with multiple error factors according to the present invention.

[0071] Figure 3 This represents the estimation error of δψ and δk in the simulation experiment.

[0072] Figure 4 This is a comparison of the positional end-point errors in the simulation experiment.

[0073] Figure 5 For comparison of positioning results.

[0074] Figure 6 For comparison of positioning errors. Detailed Implementation

[0075] To address the problems of inaccurate statistical step size model estimation and deviation between heading and pedestrian orientation in conventional PDR methods under complex motion modes, this invention proposes a pedestrian integrated navigation method based on a multi-error factor tightly coupled model (MetCM). The features of this invention are as follows:

[0076] First, this invention considers step size scaling factor error, additive motion orientation error, and additive position error. A coupled model of the three errors, MetCM, is derived through the position estimation equation. Observational analysis shows that all state terms of MetCM can be observed synchronously under the condition of only position observation.

[0077] Secondly, in order to compensate for the deviation angle between the heading and the azimuth, this invention derives a more real-time iterative principal component analysis method based on the classical principal component analysis method, and uses a Kalman filter to suppress noise in the eigenvectors, thereby improving the compensation accuracy.

[0078] Finally, this invention proposes a new paradigm of integrated navigation based on MetCM / INS. This system combines the accuracy characteristics of both systems on both long and short time scales, and constructs a Kalman filter to achieve mutual calibration, effectively improving the positioning accuracy of autonomous navigation.

[0079] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0080] Figure 2 The specific implementation flow of the pedestrian integrated navigation method based on a multi-error-factor tightly coupled model of the present invention is shown, as follows: Figure 2 As shown, it includes the following steps:

[0081] Step S1: Data Acquisition.

[0082] The IMU is strapped to the chest and outputs inertial measurement data at a set frequency. The inertial measurement data is collected and recorded.

[0083] This invention straps the IMU to the chest, and compared to the classic foot-mounted pedestrian inertial navigation system, the chest-mounted navigation system is more wearable and can effectively improve the movement flexibility of pedestrians wearing the autonomous navigation system.

[0084] Step S2: Inertial Navigation System Update Process.

[0085] In this step, inertial measurement data is input into the inertial navigation system (INS) for inertial navigation calculation, updating the INS position at time k. speed and quaternions This step corresponds to Figure 2 The green box in the figure illustrates the same implementation process as existing technologies that update the inertial navigation system's position, velocity, and quaternions based on inertial measurement data.

[0086] Specifically, if the angular velocity and acceleration output by the IMU sensor at time k in the carrier coordinate system (right front upper coordinate system, abbreviated as b system) are respectively and Choose the quaternion at time k Velocity in the navigation coordinate system (north-south coordinate system, abbreviated as n-system) Position in the n-system As parameters of the inertial navigation system, the quaternion, velocity, and INS position of the inertial navigation system are updated according to the IMU output and the following update formula (1):

[0087]

[0088] In the formula, I is the identity matrix, and g n This represents the projection of gravitational acceleration over the northeastern sky system, with a typical value of approximately [009.8]. T Δt is the sampling period, and [·]× is the fourth-order antisymmetric operator. Let be the direction cosine matrix of the rotation from the b system to the n system at time k.

[0089] Step S3: MetCM position update method based on iterative principal component analysis.

[0090] The difference between this step and existing technologies lies in determining the angular error ψ between the heading and the travel direction based on iterative principal component analysis. comp , using ψ comp The heading angle in the MetCM position update formula is corrected to obtain the compensated MetCM position.

[0091] The following section first describes the MetCM position update process using step S3.1, and then describes the angular error ψ between the heading and the travel direction using step S3.2. comp The process of obtaining the MetCM position update formula in step S3.1 is modified to obtain the MetCM position update and supplementary formula (4').

[0092] S3.1: MetCM Location Update Process

[0093] This step corresponds to Figure 2 The yellow box in the middle.

[0094] Step size l and heading angle are key elements for MetCM position updates.

[0095] For the heading angle ψ, the heading angle measurement at time k It can be derived from the quaternion at time k. It is calculated, and the calculation formula is as follows:

[0096]

[0097] For the quaternion update method, please refer to formula (1) in step S2.

[0098] For step length l, this invention uses the Weinberg step length model to calculate the single-step step length measurement of a pedestrian. The model is defined as follows:

[0099]

[0100] Based on the above model, the MetCM location update process in this step is as follows:

[0101] 1) Obtain acceleration data from inertial measurement data The modulus length is determined and the modulus length sequence is low-pass filtered.

[0102] 2) Divide the single-step interval of pedestrians according to the peak detection method or the zero-crossing axis method;

[0103] 3) Search for the maximum value f of the acceleration modulus within a single step interval. max and minimum value f min ;

[0104] 4) Set the step size ratio parameter K, and calculate the measured value of the single step size according to formula (3).

[0105] 5) When the pedestrian's single-step movement ends, at the update point, the quaternion q of INS is converted into the heading angle according to equation (2). Update the MetCM position at time k according to the following formula (4).

[0106]

[0107] Here, Δ represents a single-step period, and one period includes multiple moments. k-Δ represents the end time of the previous single-step period, and moment k is the end time of the current single-step period. The position measurement of the MetCM at time k-Δ is given. The heading angle measurement at time k-Δ is... This represents the single-step step length measurement at time k-Δ.

[0108] Under mixed conditions of sidewalking, backward walking, etc., when the pedestrian's movement direction is inconsistent with the heading, directly using equation (4) to update the position is prone to introducing large errors. Therefore, the present invention adds compensation for the heading angle ψ. The compensation method is shown in step S3.2.

[0109] S3.2: Heading / Azimuth Deflection Compensation Based on Iterative Principal Component Analysis.

[0110] This step corresponds to Figure 2 The blue box indicates that there is a horizontal acceleration data sequence from time k-Δ to time k at time k [f]. k f k-1 ...f k-Δ The covariance matrix J at time k using classical principal component analysis. k The calculation formula is as follows:

[0111]

[0112] Expand J k And order J can k The solution, expressed in iterative form, yields:

[0113]

[0114] In the formula, For matrix J k Maximum eigenvalue λ max corresponding feature vector The first equation in equation (6) represents the iterative calculation of the covariance matrix of the uncentered acceleration sequence, the second equation is the iterative calculation of the mean of the acceleration sequence, the third equation uses the mean to center the covariance matrix, and finally the eigenvectors are obtained. Since Equation (6) transforms the calculation of the covariance matrix and the mean vector into an iterative form, this method significantly reduces the computational cost at the update time.

[0115] Feature vector The angular relationship with the forward axis of the b-frame is an estimate of the angular error between the heading angle and the direction of travel.

[0116] Design a Kalman filter to suppress... The noise can be used to obtain the eigenvector estimate η. k .

[0117] In a preferred embodiment, taking into account the influence of human movement characteristics, under the same gait movement conditions... The noise is relatively large, and under this condition, the corresponding true value (or state) η k It should tend towards a relatively stable constant value, while taking into account η under different gaits. k There are significant differences, in order to fully consider the new information For η k To mitigate the effect of forgetting, this paper utilizes a Kalman filter with a constant forgetting factor α (α = 2 in this paper) to suppress forgetting. The noise is reduced to improve the accuracy of the orientation error estimation. Then η k The estimation equation and the compensation angle at time k The calculation equation is:

[0118]

[0119] In the formula, P k Let R be the Kalman filter covariance matrix at time k. k To observe the noise covariance matrix, K k For the filter gain, I i Let be the i-th order identity matrix.

[0120] η can be estimated using equations (6) and (7). k The specific process is as follows: Keep all sensitive axes of the IMU aligned with the b-frame. The pedestrian's orientation on the horizontal plane of the b-frame is defined as η. ref =

[01] T If the pedestrian walks to the right, that is, along the b line

[10] , T For vector walking, since the horizontal acceleration in the b-frame is around the x-axis, the eigenvector η can be estimated using equations (6) and (7). k The vector estimation result should be close to η. k =

[10] T Then use η ref With η k The angular error between the heading and the direction of travel at time k can be calculated using the inner product relationship between them. From a compensation perspective.

[0121] The angle at time k-Δ The compensation can be added to equation (4) in S3.1. After the compensation is added, equation (4) becomes the MetCM position. The update and compensation formulas are as follows:

[0122]

[0123] The following steps involve All The compensated heading angle.

[0124] Steps S4-S5: Design of MetCM / INS integrated navigation system.

[0125] Step S4: MetCM modeling.

[0126] If there is and (δk k δ is the step size proportional error factor; δ· is the error term; (For the measured value), it can be seen from equation (4) of S3.1 that the initial position error is: δψ from time 0 to k 0:k and δk0:k Together they influenced δk and δψ do not exhibit consistency under conditions such as mismatch between the step size model and the user or changes in gait. When the two are coupled into δp n The integration process leads to poor autonomous positioning accuracy of the navigation system over long time scales. From the perspective of direct integrated navigation, the continuously increasing δp n This causes it to fail to meet the Kalman filter observation noise assumption, thereby reducing the filtering accuracy.

[0127] Therefore, this invention transforms position estimation into position error estimation and couples it with δk and δψ. This is essential for improving the observable dimension of system parameters under conditions where only position observation is available and for compensating the navigation system. Substituting the various errors into equation (4) S3.1 yields the discrete nonlinear state expression:

[0128]

[0129] Equation (8) is essentially a nonlinear equation concerning the error term. Directly estimating it using nonlinear filtering would introduce a large computational burden, thus requiring linearization. Considering that the step size scaling factor operates near zero and the magnetometer alignment error is generally within 10 degrees, δk can be considered... k →0、δψ k →0, then cosδψ k ≈1、sinδψ k ≈δψ k δk k δψ k ≈0, then (8) can be further simplified:

[0130]

[0131] Combining equation (9) with equation (4') in S3.1, we can obtain the following about The coupling equations:

[0132]

[0133] Let δk be the position error of the MetCM at time k-Δ. k-Δ Let δψ be the step size scaling factor error at time k-Δ. k-Δ The heading angle error at time k-Δ; This represents the single-step measurement value at time k-Δ. The heading angle calculated by quaternion at time k-Δ and then compensated.

[0134] Equation (10) is δψ k and δkk The coupling equation, and this equation constitutes Self-iteration.

[0135] If we use a Kalman filter to estimate δψ k and δk k Therefore, both need to be modeled. The models differ depending on the application scenario: 1) When the system is in a state of heading inaccuracy or the step size model is not calibrated, then δψ k and δk k The main components are constant values ​​with relatively large values ​​and small random errors, so they can be modeled as random constants; 2) When condition 1) is satisfied, the heading and step length errors have a certain degree of randomness. Considering that there is a certain correlation between the gait of adjacent single steps when a pedestrian moves, and that adjacent gaits cannot be completely identical, it can be modeled as a random walk process. Therefore, δψ can be obtained. k and δk k The model is as follows:

[0136]

[0137] Among them, w δk,k-Δ and w δψ,k-Δ These are noises that satisfy a normal distribution. and These are the variances of the defined normal distribution.

[0138] Equations (10) and (11) constitute the MetCM model: Equation (10) represents the MetCM position error δp. n The coupling equations with step size scaling factor error δk and heading angle error δψ; Equation (11) is the modeling of step size scaling factor error δk and heading angle error δψ.

[0139] S5: Design a Kalman filter by combining the MetCM model and the INS error model, estimate the position error, compensate to the INS position and MetCM position, and output the calibrated position.

[0140] Compared to MetCM, which is based on a statistical step-size model, INS provides richer motion information representation over short periods and relies on kinematic differential equations to estimate the attitude, velocity, and position of the pedestrian navigation system. This results in higher single-step size estimation accuracy under conditions of sufficient convergence. However, a drawback of INS is that it may diverge relatively quickly if left uncorrected for extended periods. In terms of integration order, MetCM has a lower integration order than INS and exhibits better stability over longer time scales. Therefore, combining MetCM and INS can improve positioning accuracy.

[0141] This invention selects the INS misalignment angle φ and the velocity error δv.n Position error The positional error δp of MetCM n The error states of the integrated navigation system are constructed by stacking the step size scale factor error δk in sequence. The coupling characteristics are fully considered. Given the consistency of the heading errors of MetCM and INS, therefore δψ k and φ k =[φ E,k φ N,k φ U,k ] T The upward attitude error φ U,k They share the same error state. The subscripts of φ represent east, north, and celestial directions, respectively. U,k Let be the attitude error. Therefore, the discrete-form state expression of the integrated navigation system can be obtained as follows:

[0142]

[0143] In the formula, I i Let O be an i-order identity matrix. i*j Let f be a zero matrix with row i and column j. n × is a third-order antisymmetric matrix composed of acceleration vectors in the n-system. M / I,k Let t be the state at time k, and Δt be the sampling period.

[0144] Φ M / I,k|k-1 Here is the state transition matrix. These are single-step step length measurements, obtained based on inertial navigation data. The heading angle is a measured value, obtained by quaternion calculation and compensation.

[0145] In order to ensure that the INS error equation can be updated synchronously with MetCM, this invention addresses the issue of Φ. 41 and Φ 44 The single-step measurement is divided by the single-step period to approximate the mileage increment of the MetCM in a single sampling period (the frequency of the Kalman filter time update in this paper is equivalent to the sampling frequency).

[0146] If the position measurement of INS at time k is And constitute Then we have the observation vector Obtained from formula (1), Obtained from formula (4'); therefore, the observation equation of the integrated navigation system can be obtained as follows:

[0147]

[0148] Based on the above state equation and observation equation, Kalman filter measurement updates are performed to obtain the state estimation result, in which the MetCM position error δp is included. n and INS position error Similarly, the MetCM position error δp n Or INS position error Compensate to the INS and MetCM positions, and output the calibrated position.

[0149] The overall execution flow of the navigation system is as follows: Figure 2 As shown, inertial measurement data is input into the INS, and updated using the iterative principal component analysis method and Kalman filter time update module. The single-step detector detects the single-step period based on the periodicity of the acceleration data. When the single-step detector detects the end of the pedestrian's single-step movement, it triggers the MetCM position update. Then, based on the position results of MetCM and INS, the observation vector is constructed and updated using Kalman filter measurement. Finally, the estimation results are compensated to the INS and MetCM, and the position information is output.

[0150] The application methods and effects of the present invention are described below.

[0151] (1) Simulation Experiment

[0152] The simulation experiment was conducted using a laptop equipped with an Intel i9-14900HX processor and 1000 Monte Carlo simulations. It was assumed that a pedestrian walked 100 meters eastward at a constant speed in a straight line. The single sampling of δψ, the single sampling of δk, the eastward white noise observed at the location, and the northward white noise observed at the location all followed a normal distribution with a mean of 0 and a variance of 0.04. Considering that the estimation of δψ and δk should have a certain order, this paper estimates δψ first and then δk. The experimental results are as follows... Figure 3 and 4 As shown. In Figure 3 In the results, the mean estimation error of δψ is 0.046 degrees, and the maximum absolute value is 3.643 degrees; the mean estimation error of δk is 8.873e-05, and the maximum absolute value is 0.002. The estimation results show that the overall estimation accuracy of both is good. However, due to the noise in the position observations, the implicit heading observation noise is amplified between adjacent steps, resulting in a larger estimation error of δψ. Considering the long-term variability of position observations, i.e., calculating the angle of the walking trajectory relative to the observation, may further reduce the estimation error of δψ.

[0153] Figure 4 The end-position error of the PDR method and the end-position error of MetCM were compared. Figure 4It can be seen that the end position error of the PDR method is widely distributed, while the end position error of the MetCM-based navigation method is basically concentrated near the zero point, which verifies the impact of the estimation results of δψ and δk on navigation accuracy.

[0154] The simulation navigation results also indirectly verify the conclusion that δψ and δk are observable under the premise of only position observation.

[0155] (2) MetCM / INS Integrated Navigation Experiment

[0156] In the MetCM / INS integrated navigation experiment, this invention designed a 1-kilometer walking experiment under complex gait conditions. This experiment included three common gait types: regular walking, lateral walking, and running. The Weinberg model was selected as the stride length model. The PDR method based on iterative principal component analysis (Method 2) and the MetCM / INS integrated navigation system (which includes iterative principal component analysis, abbreviated as the Proposed Method) were compared with the PDR method based on classical principal component analysis (Method 1) to hierarchically verify the improvement effect of the proposed method on the system. The experiment used data from the first 100 meters and RTK offline calibration of δψ and δk, then compensated the calibration results into the system and restarted position calculation. The experimental results are as follows: Figure 5 and Figure 6 As shown.

[0157] Depend on Figure 5 and Figure 6 It can be seen that the iterative principal component analysis method proposed in this invention follows the general trend of classical methods, but the iterative principal component analysis method has higher positioning accuracy. Figure 5 It is not difficult to observe that after RTK calibration, the calculated trajectory near the initial moment of the classical method deviates from the reference trajectory in the heading direction. This is because the eigenvectors estimated by the classical principal component analysis method contain certain random noise, resulting in poor compensation. The iterative principal component analysis method transforms the classical principal component analysis method into an iterative form and uses a Kalman filter to reduce noise. From the derivation results, it is easy to see that its filtering result is equivalent to taking the average of multiple eigenvector results. Therefore, the iterative principal component analysis method has higher accuracy than the classical principal component analysis method. Relying solely on the principal component analysis method to compensate for the error between the motion direction and the heading is insufficient. Based on the MetCM / INS error model, the accuracy advantages of different navigation models are combined to further improve the positioning accuracy of the navigation system. Compared with the classical method, the proposed method improves the positioning accuracy by approximately 63.82%, 66.82%, and 35.22% in terms of mean, maximum, and terminal errors, respectively. If the positioning accuracy is measured by the maximum error, the proposed method achieves a positioning accuracy of 1.35%.

[0158] The specific embodiments described above only illustrate the design principles of the present invention. The shapes and names of the components in this description may differ and are not limited. Therefore, those skilled in the art can modify or make equivalent substitutions to the technical solutions described in the foregoing embodiments; and these modifications and substitutions do not depart from the inventive spirit and technical solutions of the present invention, and should all fall within the protection scope of the present invention.

Claims

1. A pedestrian integrated navigation method based on a tightly coupled multi-error-factor model, characterized in that, include: Step 1: Strapdown IMU to chest to collect inertial measurement data; Step 2: The INS performs inertial navigation calculations based on inertial measurement data and updates... k Moments' Instagram location Speed ​​and quaternions; Step 3: Determine the inertial measurement data in k Covariance matrix at time step J k Solution J k The eigenvector corresponding to the largest eigenvalue Using feature vectors The angular relationship with the forward axis of the carrier coordinate system is used to determine the angular error between the heading and the travel direction under multi-gait conditions. ; The single-step detector detects the step length based on inertial measurement data. When the pedestrian's single-step movement ends, the quaternion is converted into a heading angle. and update k Timing of MetCM position based on a tightly coupled model with multiple error factors And using the angular error between the heading and the direction of travel. Provide compensation; The angular error between the heading and the direction of travel is used. When performing compensation, the location of MetCM The update and compensation formulas are as follows: in, for MetCM position at time. for The heading angle calculated based on quaternions at any given moment. for The angular error between the heading and the direction of travel at any given time. for The measured step length at any given time; Indicates a single-step period. Indicates the end time of the previous single-step cycle; Step 4: MetCM Modeling: The MetCM model includes the MetCM location error. Step size scaling factor error and heading angle error Three models; Transform the position estimate into a position error estimate form, and convert the MetCM position error... Modeled as error with step size scaling factor and heading angle error Coupled equations, for step size scaling factor error and heading angle error Modeling; Step 5: Design a Kalman filter by combining the MetCM model and the INS error model, estimate the position error, compensate to the INS position and MetCM position, and output the calibrated position.

2. The method as described in claim 1, characterized in that, In step 3, the angular error between the heading and the walking direction under multi-step conditions is determined. The method is as follows: Based on acceleration data from inertial measurement data, a horizontal acceleration data sequence is constructed using principal component analysis. k Covariance matrix at time step J k The iterative solution algorithm is used to solve the problem. J k The eigenvector corresponding to the largest eigenvalue ; Kalman filtering suppression To eliminate noise and obtain eigenvector estimates ; The IMU's sensing axes are kept aligned with the carrier coordinate system, and the pedestrian's orientation on the horizontal plane of the carrier coordinate system is defined as the reference direction. ;use and Find the inner product relationship between them The angular relationship with the forward axis of the carrier coordinate system is the angular error between the heading and the direction of travel. .

3. The method as described in claim 2, characterized in that, The iterative solution algorithm is used to solve the problem. J k The eigenvector corresponding to the largest eigenvalue for: Assumption k Always Time's up k Horizontal acceleration data sequence at time points Using the classical principal component analysis method in k Covariance matrix at time step J k The calculation formula is as follows: In the formula Indicates a single-step period. Indicates the end time of the previous single-step cycle; Expand J k And order The covariance matrix J k Transform into an iterative computation mode: Iterative solution Then calculate J k The eigenvector corresponding to the largest eigenvalue .

4. The method as described in claim 2, characterized in that, The Kalman filter is used for suppression. The noise-reducing steps employ a constant forgetting factor. Kalman filtering.

5. The method as described in claim 4, characterized in that, Using a constant forgetting factor Kalman filtering, constructing The estimation equation is: in, for k The Kalman filter covariance matrix at time t. To observe the noise covariance matrix, For filter gain, It is a 2-order identity matrix.

6. The method as described in claim 1, characterized in that, Step 3 specifically includes: Step 31: Calculate the magnitude of the acceleration data in the inertial measurement data and perform low-pass filtering on the magnitude sequence; Step 32: Divide the pedestrian's single-step interval according to the peak detection method or the zero-crossing axis method; Step 33: Search for the maximum value of the acceleration modulus within a single step interval. and minimum value ; Step 34: Calculate the pedestrian's single step length ; Step 35: When the pedestrian's single-step movement ends, convert the quaternion of INS into a heading angle. Using the aforementioned update and compensation formula, update k MetCM position at time .

7. The method as described in claim 1, characterized in that, In step 4 The MetCM position error The model is as follows: MetCM position error Step size scaling factor error and heading angle error Substituting into the MetCM position update formula, we obtain a nonlinear equation for the error term. Linearizing this equation yields a linearized equation. Based on the fact that the step size scaling factor operates near zero and the magnetometer alignment heading error is within a small angular range, it is assumed that... k Step size scaling factor error at time step and heading angle error As the linearization equation approaches zero, the positional error of the MetCM is simplified to obtain the positional error. Error with step size scaling factor and heading angle error The coupling equation is: in, for MetCM position error at time t. for The step size scaling factor error at time step, for The heading angle error at any given time; for The single-step step length measurement at time [time]. for The heading angle at any given moment, calculated and compensated based on quaternions; The step size scaling factor error and heading angle error The model is as follows: 1) When the system is in a state of course misalignment or the step size model is not calibrated, and Modeled as random constants: 2) When condition 1) is satisfied, the heading angle error and step size scaling factor error exhibit randomness. and Modeled as a random walk process: in, and These are noises that satisfy a normal distribution. and These are the variances of the defined normal distribution.

8. The method as described in claim 1, characterized in that, Step 5 specifically includes: ① Select INS misalignment angle INS speed error INS position error MetCM position error Step size scale factor error Constructing the error state of the MetCM / INS integrated navigation system: ； ②Based on the error state Construct the state equations of the integrated navigation system: In the formula, for i An identity matrix of order 1. for i OK j The zero matrix of columns, It is a third-order antisymmetric matrix composed of acceleration vectors in the northeast-northeast coordinate system; for k The state at any given moment, The sampling period is Here is the state transition matrix. This is a single-step measurement value. The heading angle is obtained based on quaternion calculation and has been compensated. INS location and MetCM position The difference is used as the observation vector Then, the observation equations for the integrated navigation system are constructed as follows: ③ Based on the state equation and observation equation, perform Kalman filter measurement updates to obtain the state estimation result, including the MetCM position error in the state estimation result. and INS position error Similarly, the MetCM position error Or INS position error Compensate to the INS and MetCM positions, and output the calibrated position.

Images