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

Abstract

The invention provides a pedestrian integrated navigation method based on a multi-error-factor tight coupling model. The pedestrian integrated navigation method comprises the following steps: acquiring inertial measurement data by using a sensor arranged at the chest; performing inertial navigation calculation by the INS based on inertial measurement data, and updating the position of the INS; an iterative principal component analysis method is adopted to determine the angle error between the course and the walking direction under the multi-gait condition, the MetCM course based on quaternion solution is compensated, and the updated and compensated MetCM position is obtained; and constructing a Kalman filter combining the MetCM model and the INS error model, estimating a position error, compensating to an INS position and a MetCM position, and outputting a calibrated position. According to the invention, the positioning precision of autonomous navigation can be effectively improved.

Description

Technical Field

[0001] The present invention relates to the fields of navigation, guidance and control, and in particular to a pedestrian combined navigation method based on a multi-error factor tightly coupled model. Background Art

[0002] Upper-limb strapdown pedestrian dead reckoning (PDR) is a pedestrian navigation and positioning method with strong autonomy, low drift, and excellent wearability. It is widely used in a variety of fields, including individual combat, emergency rescue, and daily life. This method relies on an inertial measurement unit (IMU) to estimate the step length and heading angle, combining them to form a single-step displacement increment to update the pedestrian's position. Specifically, the IMU is strapped down to the pedestrian's upper limb, such as the waist or chest. The heading angle is continuously estimated using an inertial navigation system (INS). The pedestrian's direction of movement is assumed to be consistent with the heading. Because the inertial measurement data period and the single-step period are consistent, the peak detection method can be used to divide the single-step interval into sections. The pedestrian's single-step length is estimated based on the inertial measurement data within the section and a statistical step length model. The step length and heading are combined to calculate the pedestrian's single-step displacement increment, thereby updating the pedestrian's position information.

[0003] However, existing PDR methods have problems such as inaccurate statistical step length model estimation results and offset between heading and pedestrian motion orientation in complex motion patterns. Summary of the Invention

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

[0005] In order to solve the above technical problems, the present invention is implemented as follows.

[0006] A pedestrian integrated navigation method based on a multi-error factor tightly coupled model includes:

[0007] Step 1: Strap down the IMU to the chest to collect inertial measurement data.

[0008] Step 2: INS performs inertial navigation calculation based on inertial measurement data and updates the INS position at time k Velocity 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 maximum eigenvalue of Using feature vectors The angular relationship with the forward axis of the carrier coordinate system determines the angular error between the heading and the walking direction under multiple gait conditions

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

[0011] Step 4: MetCM modeling: Convert the position estimate into a position error estimate and convert the MetCM position error δp n The step scale factor error δk and the heading angle error δψ are modeled as coupled equations;

[0012] Step 5: Design a Kalman filter based on the MetCM model and the INS error model to estimate the position error, compensate it 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 the multi-gait condition is determined The way is:

[0014] Based on the acceleration data in the inertial measurement data, the principal component analysis method is used to construct the covariance matrix J of the horizontal acceleration data sequence at time k. k ; Use iterative solution algorithm to solve J k The eigenvector corresponding to the maximum eigenvalue of

[0015] Kalman filter suppression The noise is obtained to estimate the eigenvector η k ;

[0016] Keep the sensitive axes of the IMU 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 η ref ; Using η ref and η k The inner product relationship between them is used to find η k The angular relationship with the forward axis of the carrier coordinate system is the angular error between the heading and the walking direction.

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

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

[0019]

[0020] Where Δ represents the single-step cycle, and k-Δ represents the end time of the previous single-step cycle;

[0021] Expand J k And order The covariance matrix J k Convert to iterative calculation mode:

[0022]

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

[0024]

[0025] Iterative solution J k , then calculate J k The eigenvector corresponding to the maximum eigenvalue of

[0026] Preferably, the Kalman filter is used to suppress In the step of noise removal, a Kalman filter with a constant forgetting factor α is used.

[0027] Preferably, a Kalman filter with a constant forgetting factor α is used to construct η k The estimated 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 kis the Kalman filter covariance matrix at time k, R k is the observation noise covariance matrix, K k is the filter gain, and I2 is the second-order unit matrix.

[0032] Preferably, in step 3, the angular error between the heading and the walking direction is used When compensation is performed, the MetCM position The update and compensation formula is:

[0033]

[0034] in, is the position of MetCM at time k-Δ, is the heading angle calculated based on quaternion at time k-Δ, is the angular error between the heading and the walking direction at time k-Δ, is the single-step step length measurement value at time k-Δ; Δ represents the single-step cycle, and k-Δ represents the end time of the previous single-step cycle.

[0035] Preferably, the step 3 specifically includes:

[0036] Step 31: Obtain the modulus of acceleration data in the inertial measurement data and perform low-pass filtering on the modulus 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 the single-step interval max and the minimum value f min ;

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

[0040] Step 35: When the pedestrian's single-step movement is detected, the INS quaternion is converted into a heading angle , using the update and compensation formula, update the MetCM position at time k

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

[0042] The MetCM model includes the MetCM position error δp n , step scale factor error δk and heading angle error δψ three models; among them,

[0043] The MetCM position error δp n Modeled as:

[0044] The MetCM position error δp n , step scale factor error δk and heading angle error δψ are substituted into the MetCM position update formula to obtain a nonlinear equation about the error term, which is linearized to obtain a linearized equation;

[0045] Based on the fact that the step scale factor works near zero and the magnetometer heading error is within a small angle range, it is considered that the step scale factor error δk at time k k and heading angle error δψ k tends to zero, and the linearized equation is simplified to obtain the MetCM position error δp n The coupling equations with the step scale factor error δk and the heading angle error δψ are:

[0046]

[0047] in, is the MetCM position error at time k-Δ, δk k-Δ is the step size proportional factor error at time k-Δ, δψ k-Δ is the heading angle error at time k-Δ; is the single-step measurement value at the k-Δ moment, is the heading angle after quaternion solution and compensation at the k-Δ moment;

[0048] The step scale factor error δk and heading angle error δψ are modeled as follows:

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

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

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

[0052] 2) When condition 1) is met, the heading angle error and step scale factor error are random, and δψ k and δk k Modeled as a random walk process:

[0053]

[0054] Among them, w δk,k-Δ and w δψ,k-Δ are the noises that satisfy the normal distribution, and are the variances of the normal distributions set respectively.

[0055] Preferably, the step 5 specifically includes:

[0056] ① Select INS misalignment angle φ and INS velocity error δv n , INS position error MetCM position error δp n , the step scale factor error δk constructs the error state of the MetCM / INS integrated navigation system:

[0057]

[0058] ②According to the error state x M / I Construct the state equation of the integrated navigation system:

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

[0060]

[0061] Where, I i is the i-order identity matrix, O i*j is a zero matrix with i rows and j columns, f n × is the third-order antisymmetric matrix composed of acceleration vectors in the northeast celestial coordinate system; M / I,k is the state at time k, Δt is the sampling period, Φ M / I,k|k-1 is the state transition matrix, is the single-step measurement value, The heading angle is obtained based on quaternion solution and compensated;

[0062] Set INS position and MetCM location The difference between Then construct the observation equation of 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, the Kalman filter measurement is updated to obtain the state estimation result. The MetCM position error δp in the state estimation result is n and INS position error Similarly, the MetCM position error δp n or INS position error Compensate to the INS position and MetCM position and output the calibrated position.

[0065] Beneficial effects:

[0066] (1) The present invention adopts a combined navigation model of MetCM+INS. Compared with MetCM based on the statistical step-length model, INS has richer motion information representation in a short period of time and relies on the kinematic differential equation to estimate the attitude, velocity and position of the pedestrian navigation system, so that its single-step step-length estimation accuracy is higher under the condition of sufficient convergence. However, the defect of INS is that if it is not corrected for a long time, it may cause it to diverge quickly. From the perspective of the integration order, the integration order of MetCM is smaller than that of INS and has better stability on a longer time scale. Therefore, the present invention combines MetCM and INS to improve positioning accuracy.

[0067] (2) Considering that in mixed gait conditions such as sideways walking and backwards, when the pedestrian's movement direction is inconsistent with the heading, directly updating the position is likely to introduce large errors. Therefore, the present invention compensates for the heading angle to further improve the positioning accuracy.

[0068] (3) In a preferred embodiment, the iterative principal component analysis method converts the classical principal component analysis method into an iterative form and uses a Kalman filter to reduce noise. From the derivation results, it is clear that the 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 calculation required at the update time. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 Flowchart of the prior art PDR method.

[0070] Figure 2 This is a flow chart of the pedestrian integrated navigation method based on the multi-error factor tightly coupled model of the present invention.

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

[0072] Figure 4 Comparison of position end errors in simulation experiments.

[0073] Figure 5 For comparison of positioning results.

[0074] Figure 6 Comparison of positioning errors. DETAILED DESCRIPTION

[0075] To address the problems of conventional PDR methods in complex motion patterns, such as inaccurate statistical step-length model estimation results and the offset between heading and pedestrian motion orientation, this paper proposes a pedestrian integrated navigation method based on a multi-error factor tightly coupled model (MetCM). The present invention is characterized by the following points:

[0076] First, the present invention considers the step-size scale factor error, additive motion orientation error, and additive position error, and derives a coupling model of the three errors, namely, MetCM, through the position estimation equation. Observable analysis shows that all state items of MetCM can be synchronously observed under the condition of only position observation.

[0077] Secondly, in order to compensate for the deviation angle between the heading and the motion azimuth, the present invention derives an iterative principal component analysis method with stronger real-time performance based on the classical principal component analysis method, and suppresses the noise of the eigenvector through the Kalman filter, thereby improving the compensation accuracy.

[0078] Finally, the present invention proposes a new paradigm of combined navigation based on MetCM / INS. The system combines the accuracy characteristics of the two on long and short time scales, jointly constructs Kalman filters to achieve mutual calibration, and effectively improves the positioning accuracy of autonomous navigation.

[0079] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0080] Figure 2 The specific implementation process of the pedestrian combined navigation method based on the multi-error factor tightly coupled model of the present invention is shown as follows: Figure 2 As shown, the following steps are included:

[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] The present invention straps the IMU to the chest. Compared with the classic foot-strap pedestrian inertial navigation system, the chest-strap 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, the inertial measurement data is input into the inertial navigation system INS for inertial navigation solution, and the INS position at time k is updated. speed and quaternions This step corresponds to Figure 2 The green box in the figure has the same implementation process as the existing solution of updating the inertial navigation position, velocity and quaternion based on inertial measurement data.

[0086] Specifically, if the angular rate and acceleration of the carrier coordinate system (the right front upper coordinate system, referred to as the b system) output by the IMU sensor at time k are and Select the quaternion at time k Speed ​​in the navigation coordinate system (northeast celestial coordinate system, abbreviated as n system) Position in n system As the parameters of the inertial navigation system, the quaternion, velocity and INS position of the inertial navigation system are updated according to the IMU output according to the following update formula (1):

[0087]

[0088] Where I is the unit matrix, g n is the projection of gravity acceleration in the northeastern sky system, with a typical value of about [0.09.8] T , Δt is the sampling period, [·]× is the fourth-order antisymmetric operator, is the direction cosine matrix of the rotation from system b to system n at time k.

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

[0090] The difference between this step and the prior art is that the angular error ψ between the heading and the walking direction is determined based on the iterative principal component analysis method. comp , using ψ comp Correct the heading angle in the MetCM position update formula to obtain the compensated MetCM position

[0091] Next, we will first refer to step S3.1 to introduce the MetCM position update process, and then refer to step S3.2 to introduce the angular error ψ between the heading and the walking direction. comp The acquisition process is carried out, and the MetCM position update formula of step S3.1 is corrected to obtain the MetCM position update and supplement formula (4').

[0092] S3.1: MetCM Location Update Procedure

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

[0094] The step size l and heading angle are the key elements of MetCM position update.

[0095] For the heading angle ψ, the heading angle measurement value at time k The quaternion at time k The calculation formula is as follows:

[0096]

[0097] The quaternion updating method is shown in formula (1) in step S2.

[0098] For the step length l, the present invention uses the Weinberg step length model to calculate the pedestrian's single step length measurement value , 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 module length is determined and low-pass filtering is performed on the module length sequence;

[0102] 2) Divide the pedestrian's single-step interval based on the peak detection method or the zero-crossing axis method;

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

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

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

[0106]

[0107] Where Δ represents a single-step cycle, which consists of multiple moments. k-Δ represents the end time of the previous single-step cycle, and moment k is the end time of the current single-step cycle. is the MetCM position measurement value at time k-Δ, is the heading angle measurement value at time k-Δ, is the single-step measurement value at time k-Δ.

[0108] In mixed gait conditions such as sideways walking and backwards, when the pedestrian's movement direction is inconsistent with the heading, directly using equation (4) to update the position is likely to introduce 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 / motion azimuth compensation based on iterative principal component analysis method.

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

[0111]

[0112] Expand J k And order You can use J k The solution is written in iterative form, and we can get:

[0113]

[0114] Where, is the matrix J k Maximum eigenvalue λ max The corresponding eigenvector The first equation in formula (6) represents the iterative calculation of the uncentered acceleration sequence covariance matrix, 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 calculates the eigenvector Since Equation (6) transforms the calculation of the covariance matrix and the mean vector into an iterative form, this method significantly reduces the amount of calculation at the update time.

[0115] Eigenvector The angular relationship with the forward axis of the b frame is an estimate of the angular error between the heading angle and the walking direction.

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

[0117] In a preferred embodiment, considering the influence of human motion characteristics, the The noise is large, and under this condition the corresponding true value (or state) η k It should tend to a relatively stable constant value, taking into account the η under different gaits k There is a big difference, in order to fully consider the new information For η k In this paper, we use the Kalman filter with a constant forgetting factor α (α=2 in this paper) to suppress The noise can improve the accuracy of the direction error estimation. Then there is η k The estimation equation and compensation angle at time k The calculation equation is:

[0118]

[0119] Where, P k is the Kalman filter covariance matrix at time k, R k is the observation noise covariance matrix, K k is the filter gain, I i is the i-th order identity matrix.

[0120] η can be estimated by using equations (6) and (7): k The specific process is: keep the sensitive axes of the IMU coincident with the b-axis, and the pedestrian orientation on the horizontal plane of the b-axis is defined as η ref =

[01] T If the pedestrian walks to the right, he should follow line b.

[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 and η k The inner product relationship between them can be used to calculate the angular error between the heading and the walking direction at time k. as a compensation angle.

[0121] The angle at time k-Δ Compensation can be added to S3.1 formula (4). After adding compensation, formula (4) becomes MetCM position The update and compensation formula is:

[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; is the measured value), and through S3.1 formula (4), we can know that: the initial position error δψ from time 0 to k 0:k and δk0:k Jointly influenced Under conditions such as the step length model not matching the user or the gait changes, δk and δψ are inconsistent. When the two are coupled into δp n The integration process of δp makes the autonomous positioning accuracy of the navigation system poor on a long time scale. From the perspective of direct combined navigation, the increasing δp n This makes it dissatisfied with the Kalman filter observation noise assumption, thereby reducing the filtering accuracy.

[0127] Therefore, the present invention converts the position estimate into a position error estimate and couples it with δk and δψ. It is necessary to improve the observable dimension of the system parameters under the condition of only position observation and compensate the navigation system. Substituting each error into S3.1 equation (4) yields the discrete nonlinear state expression:

[0128]

[0129] Equation (8) is essentially a nonlinear equation about the error term. If it is directly estimated by nonlinear filtering, it will introduce a large amount of calculation, so it is necessary to linearize it. Considering that the step scale factor works near zero and the magnetometer heading error is generally within 10 degrees, it can be considered that δk 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 get The coupled equations are:

[0132]

[0133] is the MetCM position error at time k-Δ, δk k-Δ is the step size proportional factor error at time k-Δ, δψ k-Δ is the heading angle error at time k-Δ; is the single-step measurement value at the k-Δ moment, The heading angle calculated by the quaternion at the k-Δ moment and after compensation;

[0134] Formula (10) is δψ k and δkk The coupled equations of self-iteration.

[0135] If we use the Kalman filter to estimate δψ k and δk k In different application scenarios, the models of the two are different: 1) When the system is in heading misalignment or the step size model is not calibrated, then δψ k and δk k The main component of is a constant with a large value and a small random error, so it can be modeled as a random constant; 2) When condition 1) is met, the heading and step length errors have a certain degree of randomness. Considering that the gaits of adjacent single steps when pedestrians move have a certain correlation, and adjacent gaits cannot be completely consistent, it can be modeled as a random walk process. Therefore, we can get δψ k and δk k The model is as follows:

[0136]

[0137] Among them, w δk,k-Δ and w δψ,k-Δ are the noises that satisfy the normal distribution, and are the variances of the normal distributions set respectively.

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

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

[0140] Compared to the statistical step-size model-based MetCM, INS provides richer representation of motion information in the short term 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 sufficiently converged conditions. However, a drawback of INS is that it can quickly diverge if not corrected for an extended period. In terms of integration order, MetCM has a smaller integration order than INS and offers better stability over longer timescales. Therefore, MetCM and INS can be combined to improve positioning accuracy.

[0141] The present invention selects INS misalignment angle φ, speed error δvn , position error and the position error δp of MetCM n , step scale factor error δk and stack them in sequence to construct the error state of the integrated navigation system The coupling characteristics are fully considered. Considering that the heading errors of MetCM and INS are consistent, δψ k and φ k =[φ E,k φ N,k φ U,k ] T The celestial attitude error φ in U,k Share the same error state. The subscripts of φ represent east, north and sky directions respectively. U,k is the celestial attitude error. The discrete state expression of the integrated navigation system is as follows:

[0142]

[0143] Where, I i is the i-order identity matrix, O i*j is a zero matrix with i rows and j columns, f n × is a third-order antisymmetric matrix composed of acceleration vectors in the n-frame. M / I,k is the state at time k, and Δt is the sampling period.

[0144] Φ M / I,k|k-1 is the state transition matrix, is the single-step length measurement value, obtained based on inertial navigation data; The heading angle measurement is obtained based on quaternion solution and compensation.

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

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

[0147]

[0148] Based on the above state equation and observation equation, the Kalman filter measurement update is performed to obtain the state estimation result. The MetCM position error δp in the state estimation result n and INS position error Similarly, the MetCM position error δp n or INS position error Compensate to the INS position and MetCM position and output the calibrated position.

[0149] The overall execution process of the navigation system is as follows: Figure 2 As shown in the figure, inertial measurement data is input to the INS, and an iterative principal component analysis method and Kalman filter time update module are used for update. 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. The observation vector is then constructed based on the position results of the MetCM and INS, and the Kalman filter measurement update is performed. The estimated result is then compensated to the INS and MetCM, and the position information is output.

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

[0151] (1) Simulation experiment

[0152] The simulation experiment was conducted in the MATLAB environment of a notebook equipped with an Intel i9-14900HX processor. 1000 Monte Carlo experiments were performed. Assume that the pedestrian walks 100 meters in a straight line at a constant speed in the east direction. The single sampling of δψ, the single sampling of δk, the eastward white noise of the position observation, and the northward white noise of the position observation all obey 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 estimates δk. The experimental results are shown in Figure 2. Figure 3 and 4 As shown. Figure 3 The mean error in the δψ estimation is 0.046 degrees, with a maximum absolute value of 3.643 degrees; the mean error in the δk estimation is 8.873e-05, with a maximum absolute value of 0.002. The estimation results show that both methods have good overall accuracy. However, due to the noise in the position observations, the implicit heading observation noise is amplified between adjacent steps, resulting in a larger error in the δψ estimation. Considering the long-term variability of the position observations, that is, calculating the angle of the walking trajectory relative to the observation, could further reduce the δψ estimation error.

[0153] Figure 4 The end position error of the PDR method is compared with the end position error of MetCM. Figure 4It can be seen that the terminal position error of the PDR method is widely distributed, while the terminal position error of the navigation method based on MetCM is basically concentrated near the zero point, which verifies the influence of the estimation results of δψ and δk on the 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 combined navigation experiment of MetCM / INS, the present invention designed a 1-kilometer walking experiment under complex gaits. The experiment includes three common gaits: regular walking, lateral walking and running. The Weinberg model is selected as the step length model, and the PDR method based on the iterative principal component analysis method (Method 2) and the MetCM / INS combined navigation system (including the iterative principal component analysis method, abbreviated as Proposed Method) are compared with the PDR method based on the classical principal component analysis method (Method 1) to verify the improvement effect of the proposed method on the system in a hierarchical manner. The experiment uses the data of the first 100 meters and the RTK offline calibration of δψ and δk, and then compensates the calibration results to the system and restarts the position calculation. The experimental results are as follows Figure 5 and Figure 6 shown.

[0157] Depend on Figure 5 and Figure 6 It can be seen that the iterative principal component analysis method proposed in this invention has the same general trend as the classical method, but the iterative principal component analysis method has higher positioning accuracy. Figure 5 It is not difficult to find that after RTK calibration, the trajectory calculated by the classical method near the initial moment deviates from the reference trajectory in heading. This is because the eigenvectors estimated by the classical principal component analysis method contain a certain amount of random noise, which makes the compensation effect poor. The iterative principal component analysis method transforms the classical principal component analysis method into an iterative form and uses a Kalman filter to reduce the noise. The derivation shows that the filtering result is equivalent to taking the average of multiple eigenvectors. Therefore, the iterative principal component analysis method has higher accuracy than the classical principal component analysis method. Relying solely on principal component analysis to compensate for the error between motion direction and heading is not sufficient. Based on the MetCM / INS error model, the proposed method combines the accuracy advantages of different navigation models to further improve the positioning accuracy of the navigation system. Compared with the classical method, the positioning accuracy of the proposed method is improved by approximately 63.82%, 66.82%, and 35.22% in the mean, maximum, and terminal values, respectively. When the positioning accuracy is measured by the maximum error, the proposed method reaches 1.35%.

[0158] The above specific embodiments merely illustrate the design principles of the present invention. The shapes and names of the components described herein may vary and are not limiting. Therefore, those skilled in the art may modify or substitute equivalents for the technical solutions described in the above embodiments. Such modifications and substitutions, without departing from the inventive spirit and technical solutions of the present invention, shall fall within the scope of protection of the present invention.

Claims

1. A pedestrian integrated navigation method based on a multi-error factor tightly coupled model, characterized in that: include: Step 1: Strap down the IMU to the chest to collect inertial measurement data. Step 2: INS performs inertial navigation calculation based on inertial measurement data and updates the INS position at time k Velocity and quaternions; Step 3: Determine the covariance matrix J of the inertial measurement data at time k k , solve J k The eigenvector corresponding to the maximum eigenvalue of Using feature vectors The angular relationship with the forward axis of the carrier coordinate system determines the angular error between the heading and the walking direction under multiple gait conditions The single-step detector detects the single-step length based on the inertial measurement data. When the pedestrian's single-step movement ends, the quaternion is converted into a heading angle. And update the MetCM position at time k based on the multi-error factor tightly coupled model And the angular error between the heading and the walking direction is used make compensation; Step 4: MetCM modeling: Convert the position estimate into a position error estimate and convert the MetCM position error δp n The step scale factor error δk and the heading angle error δψ are modeled as coupled equations; Step 5: Design a Kalman filter based on the MetCM model and the INS error model to estimate the position error, compensate it to the INS position and MetCM position, and output the calibrated position.

2. The method according to claim 1, wherein In step 3, the angular error between the heading and the walking direction under the multi-gait condition is determined. The way is: Based on the acceleration data in the inertial measurement data, the principal component analysis method is used to construct the covariance matrix J of the horizontal acceleration data sequence at time k. k ; Use iterative solution algorithm to solve J k The eigenvector corresponding to the maximum eigenvalue of Kalman filter suppression The noise is obtained to estimate the eigenvector η k ; Keep the sensitive axes of the IMU 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 η ref ; Using η ref and η k The inner product relationship between them is used to find η k The angular relationship with the forward axis of the carrier coordinate system is the angular error between the heading and the walking direction.

3. The method according to claim 2, wherein The iterative solution algorithm is used to solve J k The eigenvector corresponding to the maximum eigenvalue of for: Assume that at time k there is a horizontal acceleration data sequence from time k-Δ to time k [f k f k-1 ...f k-Δ ], using the classical principal component analysis method, the covariance matrix J at time k k The calculation formula is as follows: Where Δ represents the single-step cycle, and k-Δ represents the end time of the previous single-step cycle; Expand J k And order The covariance matrix J k Convert to iterative calculation mode: m k =μ k-1 +D -1 (f k -f k-Δ+1 ) Iterative solution J k , then calculate J k The eigenvector corresponding to the maximum eigenvalue of 4. The method according to claim 2, wherein The Kalman filter is used to suppress In the step of noise removal, a Kalman filter with a constant forgetting factor α is used.

5. The method according to claim 4, wherein Use Kalman filtering with a constant forgetting factor α to construct η k The estimated equation is: K k =P k-1 (P k-1 +R k ) -1 P k =α(I2-K k )P k-1 Among them, P k is the Kalman filter covariance matrix at time k, R k is the observation noise covariance matrix, K k is the filter gain, and I2 is the second-order unit matrix.

6. The method according to claim 1, wherein In step 3, the angular error between the heading and the walking direction is used. When compensation is performed, the MetCM position The update and compensation formula is: in, is the position of MetCM at time k-Δ, is the heading angle calculated based on quaternion at time k-Δ, is the angular error between the heading and the walking direction at time k-Δ, is the single-step step length measurement value at time k-Δ; Δ represents the single-step period, and k-Δ represents the end time of the previous single-step period.

7. The method according to claim 6, wherein The step 3 specifically includes: Step 31: Obtain the modulus of acceleration data in the inertial measurement data and perform low-pass filtering on the modulus 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 f of the acceleration modulus within the single-step interval max and the minimum value f min ; Step 34: Calculate the pedestrian's single step length Step 35: When the pedestrian's single-step movement is detected, the INS quaternion is converted into a heading angle Using the update and compensation formula, update the MetCM position at time k 8. The method according to claim 1, wherein In step 4, the constructed MetCM model is: The MetCM model includes the MetCM position error δp n , step scale factor error δk and heading angle error δψ three models; among them, The MetCM position error δp n Modeled as: The MetCM position error δp n , step scale factor error δk and heading angle error δψ are substituted into the MetCM position update formula to obtain a nonlinear equation about the error term, which is linearized to obtain a linearized equation; Based on the fact that the step scale factor works near zero and the magnetometer heading error is within a small angle range, it is considered that the step scale factor error δk at time k k and heading angle error δψ k tends to zero, and the linearized equation is simplified to obtain the MetCM position error δp n The coupling equations with the step scale factor error δk and the heading angle error δψ are: in, is the MetCM position error at time k-Δ, δk k-Δ is the step size proportional factor error at time k-Δ, δψ k-Δ is the heading angle error at time k-Δ; is the single-step measurement value at the k-Δ moment, is the heading angle after quaternion solution and compensation at the k-Δ moment; The step scale factor error δk and heading angle error δψ are modeled as follows: 1) When the system is in heading misalignment or the step size model is not calibrated, δψ k and δk k Modeled as a random constant: δk k =δk k-Δ dψ k =dψ k-Δ 2) When condition 1) is met, the heading angle error and step scale factor error are random, and δψ k and δk k Modeled as a random walk process: Among them, w δk,k-Δ and w δψ,k-Δ are the noises that satisfy the normal distribution, and are the variances of the normal distributions set respectively.

9. The method according to claim 1, wherein The step 5 specifically includes: ① Select INS misalignment angle φ and INS velocity error δv n , INS position error MetCM position error δp n , the step scale factor error δk constructs the error state of the MetCM / INS integrated navigation system: ②According to the error state x M / I Construct the state equation of the integrated navigation system: x M / I,k =Φ M / I ,k |k-1 x M / I,k-1 Where, I i is the i-order identity matrix, O i*j is a zero matrix with i rows and j columns, f n × is the third-order antisymmetric matrix composed of acceleration vectors in the northeast celestial coordinate system; x M / I,k is the state at time k, Δt is the sampling period, Φ M / I,k|k-1 is the state transition matrix, is the single-step measurement value, The heading angle is obtained based on quaternion solution and compensated; Set INS position and MetCM location The difference between Then construct the observation equation of the integrated navigation system: y M / I,k =[O 2*6 I2 O 2*1 -I2 O 2*1 ]x M / I,k ③ Based on the state equation and observation equation, the Kalman filter measurement is updated to obtain the state estimation result. The MetCM position error δp in the state estimation result is n and INS position error Similarly, the MetCM position error δp n or INS position error Compensate to the INS position and MetCM position and output the calibrated position.

Images