Robust pedestrian autonomous navigation method based on motion matching factor online clustering

Abstract

The invention provides a robust pedestrian autonomous navigation method based on motion matching factor online clustering. The method comprises the following steps: S1, determining an alternative fusion moment by utilizing a sensitive angular velocity module value of a gyroscope; s2, uniformly describing foot landing motion states under different dynamic states by adopting a rigid body rotation model so as to construct a foot kinematics model; s3, establishing a foot kinematics / micro inertial navigation integrated navigation model under a Kalman filtering framework; s4, respectively constructing an innovation non-orthogonal factor and a residual factor as motion matching factors to represent the matching degree of the current foot kinematics model information and the real motion state; and S5, aiming at the alternative fusion moment, proposing a motion matching factor online clustering method to make a decision on whether inertial navigation information and kinematic model information are fused or not, so as to realize robust pedestrian autonomous navigation. According to the invention, the use convenience of the system can be improved, and the navigation positioning precision and robustness under movement at different speeds can be improved.

Description

Technical Field

[0001] The present invention belongs to the field of navigation, guidance and control, and particularly relates to a robust pedestrian autonomous navigation method based on online clustering of motion matching factors. Background Art

[0002] The zero-velocity update (ZUPT) method for foot-mounted micro inertial measurement unit (MIMU) is a common pedestrian autonomous navigation and positioning algorithm. The ZUPT algorithm believes that there is a moment when the velocity is zero when the foot lands. Therefore, a zero-velocity detector is used to detect this moment, and taking this moment as a trigger, the Kalman filtering algorithm is run to estimate and correct the inertial navigation system error, thereby suppressing the divergence of the inertial navigation system error.

[0003] In the traditional architecture, the detection accuracy of the zero-velocity detector is the key factor affecting the final positioning accuracy. Therefore, a large number of studies have been carried out on how to improve the accuracy and adaptability of the zero-velocity detector, and remarkable results have been achieved. However, such solutions focus on optimizing the zero-velocity detector by using prior human motion knowledge or a large amount of historical data, and there is still room for improvement in terms of convenience, robustness and adaptability. Summary of the Invention

[0004] In view of this, the present invention provides a robust pedestrian autonomous navigation method based on online clustering of motion matching factors, which can improve the convenience of system use, as well as the navigation and positioning accuracy and robustness under different speed motions.

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

[0006] A robust pedestrian autonomous navigation method based on online clustering of motion matching factors includes:

[0007] Step S1: The micro inertial measurement unit MIMU is fixed to the heel, and the gyroscope is used to sense the angular velocity modulus value to determine the alternative fusion moment;

[0008] Step S2: A rigid body rotation model is used to uniformly describe the foot landing motion state under different dynamics to construct a foot kinematic model;

[0009] Step S3: A foot kinematics / micro inertial navigation integrated navigation model is established under the Kalman filtering framework;

[0010] Step S4: According to the deviation of the observation information of the Kalman filter and the innovation no longer being orthogonal, an innovation non-orthogonal factor mm for measuring the orthogonality of the innovation in the foot kinematics / micro inertial navigation integrated navigation model is constructed. in,k; When there is no deviation in the observation information, the residual is zero, and a residual factor mm is constructed to measure the magnitude of the residual of the foot kinematics / micro inertial navigation integrated navigation model re,k ; The innovation non-orthogonal factor mm in,k and / or the residual factor mm re,k is used as the motion matching factor mm k , to characterize the matching degree between the current foot kinematics model information and the true motion state;

[0011] Step S5: For the alternative fusion moment, make an online clustering decision according to the motion matching factor mm k . When the matching degree characterized by the motion matching factor mm k reaches the set requirement, in the update process of the Kalman filter, use the foot kinematics model information to correct the inertial navigation error, so as to fuse the foot kinematics model information and the micro inertial navigation information obtained by the MIMU to achieve pedestrian autonomous navigation.

[0012] Preferably, in step S1, the method for determining the alternative fusion moment is: using rising edge trigger judgment. When the modulus of the angular velocity sensed by the gyroscope at time k - 1 is less than the set quasi-zero speed threshold γ foot , and at the same time, the modulus of the angular velocity sensed by the gyroscope at time k is greater than the set quasi-zero speed threshold γ foot , it is determined that time k is the alternative fusion moment.

[0013] Preferably, the value range of the quasi-zero speed threshold γ foot is 2 - 4 rad / s.

[0014] Preferably, in step S2, the constructed foot kinematics model is:

[0015] When the foot is in the landing motion:

[0016]

[0017] where n is the navigation coordinate system, is the velocity vector of the foot in the navigation coordinate system at time k; b is the carrier coordinate system, is the lever arm vector from the measurement point to the landing point, is the rotational angular velocity of the measurement point, is the attitude matrix of the measurement point.

[0018] Preferably, in step S3, the constructed foot kinematics / inertial navigation integrated navigation model is:

[0019] Based on the foot kinematics model established in step S2 and the inertial navigation error equation, establish the ankle-calf kinematics / inertial navigation integrated navigation model:

[0020]

[0021] wherein i.e., using the position error δp of the inertial navigation k , velocity error δv k and attitude error δφ k as system variables; Z k = δv k i.e., using the velocity error as the observable quantity, calculated based on the velocity pseudo-measurement provided by the foot kinematic model i.e., is the velocity solved by the MIMU; F k,k-1 is the state transition matrix, Γ k-1 is the process noise driving matrix, W k-1 is the process noise, H k is the observation matrix, r k is the observation noise;

[0022]

[0023] wherein is the output value of the accelerometer in the navigation coordinate system, T s is the sampling period of the MIMU; I 3×3 is the 3×3 identity matrix, O 3×3 is the 3×3 zero matrix.

[0024] Preferably, in step S4, the innovation non-orthogonal factor is:

[0025]

[0026] The residual factor is:

[0027]

[0028] wherein, Z k is the observable quantity, H k is the observation matrix, R k is the measurement noise variance, is the one-step predicted state, P k,k-1 is the error covariance matrix of the one-step predicted state, is the estimated state at time k.

[0029] Preferably, step S5 includes:

[0030] Updating the online clustering center value

[0031]

[0032] wherein is the center value of the normal matching class, is the central value of the abnormal matching class is the number of normal matching classes is the number of abnormal matching classes; mm k-1 is the value of the motion matching factor, selecting the innovation non-orthogonal factor or the residual factor; the subscripts k and k - 1 represent moments

[0033] Calculate the matching degree of the foot kinematic model:

[0034]

[0035] where is the distance between the motion matching factor and the center of the normal matching class is the distance between the motion matching factor and the center of the abnormal matching class

[0036] If the matching degree D mm,k does not reach the set matching degree threshold, then the inertial navigation information and the foot kinematic model information are not fused at this moment. The Kalman filter only performs time update, does not perform measurement update, and does not perform inertial navigation error correction either

[0037] If the matching degree D mm,k reaches the set matching degree threshold, then the inertial navigation information and the foot kinematic model information are fused at this moment: The Kalman filter performs time update and measurement update, calculates the velocity error observation using the foot kinematic model information and then estimates the filtering state Meanwhile, use to perform inertial navigation error correction

[0038] Beneficial effects:

[0039] (1) The present invention proposes a robust pedestrian autonomous navigation method based on online clustering of motion matching factors, simplifies the system design process, and effectively improves the navigation and positioning accuracy and robustness under different speed motions

[0040] (2) Using a large-threshold quasi-zero speed rough detector based on the angular velocity modulus effectively reduces the difficulty of parameter adjustment: Only the angular velocity sensed by the gyroscope is used as the main information source of the detector to avoid the negative impact brought by introducing acceleration information; In order to ensure that there is at least one alternative fusion moment within each gait cycle under changing dynamics, the detector threshold is set to a relatively large value; In order to reduce the number of abnormal fusion moments and at the same time ensure that the normal fusion moments are in the decay stage of the angular velocity, the rising edge trigger correction method is adopted

[0041] (3) Constructing the innovation non-orthogonal factor and the residual factor as the motion matching factors can effectively characterize the matching degree between the foot kinematic model information and the real motion state

[0042] (4) A robust fusion algorithm based on online clustering decision is proposed to improve the adaptability of the decision to the motion matching factor and the filtering parameter. Description of the Drawings

[0043] Figure 1 It is a schematic diagram of a robust pedestrian autonomous navigation method based on online clustering of motion matching factors.

[0044] Figure 2 It is a schematic diagram of the motion state during the foot landing phase. Detailed Implementation Manner

[0045] The present invention will be described in detail below in conjunction with the drawings and by way of examples.

[0046] The present invention proposes a robust pedestrian autonomous navigation method based on online clustering of motion matching factors.

[0047] As Figure 1 shown, it includes the following steps:

[0048] Step S1: Use a simple large-threshold quasi-zero velocity rough detector to obtain alternative fusion moments.

[0049] Step S2: Use a rigid body rotation model to uniformly describe the foot landing motion state under different dynamics, and construct a foot kinematic model to provide velocity pseudo-measurement information for the subsequent steps.

[0050] Step S3: Establish a foot kinematics / micro-inertial navigation integrated navigation model under the Kalman filtering framework to provide a system model and an observation model for subsequent motion matching and fusion decision-making.

[0051] Step S4: Respectively construct an innovation non-orthogonal factor and a residual factor as motion matching factors to characterize the matching degree between the current foot kinematic model information and the real motion state, and provide supporting information for subsequent fusion decision-making;

[0052] Step S5: For the alternative fusion moments, propose an online clustering method for motion matching factors to make a decision on whether to fuse the inertial navigation information and the kinematic model information, so as to achieve robust pedestrian autonomous navigation.

[0053] The implementation steps of the present invention will be described in detail below.

[0054] Step S1: Design of a large-threshold quasi-zero velocity rough detector based on the modulus of the angular velocity sensed by the gyroscope.

[0055] The present invention adopts a large-threshold quasi-zero velocity rough detector based on the modulus of the angular velocity, and uses the rising edge trigger correction method. It does not require the use of a sliding window and has the characteristics of simplicity, high efficiency, and ease of use in design and use.

[0056] First, as the main functional unit during human walking, the foot has rich linear and angular motion information. Therefore, a MIMU (Micro Inertial Measurement Unit) containing an accelerometer and a gyroscope can be used to sense the foot motion information. However, when the dynamic state of human forward movement becomes larger, a large impact will be exerted on the foot when it touches the ground. At this time, the acceleration signal changes violently, and using the acceleration information to construct a detector will have a negative impact. Therefore, the present invention only uses the gyroscope to sense the angular velocity as the main information source of the detector.

[0057] Secondly, in order to ensure that there is at least one alternative fusion moment within each gait cycle under varying dynamic states, the detector threshold is set to a relatively large value. At the same time, for the sake of simplicity in calculation, the window is set to 1. The detector threshold γ foot is preferably 2 - 4 rad / s.

[0058] Meanwhile, in order to reduce the number of abnormal fusion moments and ensure that the normal fusion moments are in the decay stage of the angular velocity, the method of triggering and correcting at the rising edge is adopted. When the norm of the angular velocity sensed by the gyroscope at the (k - 1)th moment is less than the set quasi-zero velocity threshold γ foot , and at the same time, the norm of the angular velocity sensed by the gyroscope at the kth moment is greater than the set quasi-zero velocity threshold γ foot , the kth moment is determined as an alternative fusion moment. That is, when the following formula is satisfied, it is used as an alternative fusion moment:

[0059]

[0060] where is the angular velocity sensed by the gyroscope at the kth moment, and γ foot is the set quasi-zero velocity threshold.

[0061] Step S2: Construct a foot kinematic model.

[0062] As Figure 2 shown, the Micro Inertial Measurement Unit (MIMU) is fixed at point M on the heel. H is the landing point, and T is the toe. The present invention uses a rigid body rotation model to uniformly describe the foot landing motion state under different dynamic states, that is, the motion from the foot touching the ground until the foot is completely in contact with the ground is modeled as MHT. The formed "L"-shaped rigid body rotates around point H. The faster the human body moves, the greater the angular velocity. At this time, the foot kinematic model is:

[0063] When the foot is in the landing motion:

[0064]

[0065] Expressed as the model at the kth moment:

[0066]

[0067] where n is the navigation coordinate system, is the velocity vector of the foot at time k in the navigation coordinate system; b is the vehicle coordinate system, is the lever arm vector from the measurement point to the touchdown point, is the rotational angular velocity of the measurement point, is the attitude matrix of the measurement point.

[0068] Step S3: Construct a foot kinematics / MINS integrated navigation model.

[0069] In the framework of Kalman filtering, the present invention establishes an ankle-calf kinematics / INS integrated navigation model based on the kinematic model and the INS error equation established in Step S2:

[0070]

[0071] where that is, the position error, velocity error, and attitude error of the INS are used as system variables; Z k = δv k that is, the velocity error is used as the observation variable, which is calculated according to the velocity pseudo-measurement provided by the kinematic model in actual use, that is is the velocity solved by the MIMU; F k,k-1 is the state transition matrix, Γ k-1 is the process noise driving matrix, W k-1 is the process noise, H k is the observation matrix, r k is the observation noise.

[0072] In Equation (3):

[0073]

[0074] where is the output value of the accelerometer in the navigation coordinate system, T s is the sampling period of the Micro Inertial Measurement Unit (MIMU); I 3×3 is a 3×3 identity matrix, O 3×3 is a 3×3 zero matrix.

[0075] Step S4: Design a motion matching factor.

[0076] The alternative fusion times obtained in S1 are only the results of rough screening using the angular velocity. If the foot kinematic model information and the MINS information are directly fused at the alternative times, large errors will occur. Therefore, the present invention proposes to use a motion matching factor to characterize the matching degree between the current kinematic model information and the real motion state, calculate the matching degree according to the motion matching factor, and make an adaptive decision on whether to fuse.

[0077] The motion matching factor refers to a scalar or vector used to characterize the matching degree between the kinematic model and the true motion state. By analyzing the information characteristics of the Kalman process, the present invention constructs the innovation non-orthogonal factor and the residual factor respectively as the motion matching factors, and comprehensively calculates the Euclidean distance between the observed information and the predicted information to characterize the matching degree between the current kinematic model information and the true motion state. If the value of the motion matching factor is lower at this time, the matching degree is higher:

[0078]

[0079] where mm in,k is the innovation non-orthogonal factor, mm re,k is the residual factor, Z k is the observed quantity, H k is the observation matrix, R k is the measurement noise variance, is the one-step predicted state, P k,k-1 is the error covariance matrix of the one-step predicted state, is the estimated state.

[0080] The construction and derivation of the innovation non-orthogonal factor and the residual factor are as follows:

[0081] (1) For the innovation non-orthogonal factor:

[0082] In ideal Kalman filtering, the innovation has orthogonality. When there is a deviation in the observed information, the innovation will no longer have orthogonality.

[0083] The innovation vector d k can be expressed as:

[0084]

[0085] where, V k is the observation noise, is the one-step prediction error, is the one-step predicted observation

[0086] So

[0087]

[0088] where, E(Z k Z k T ) is the variance of the observed value.

[0089] According to the orthogonality theory of the innovation, there is:

[0090]

[0091] At this time:

[0092]

[0093] When the observed information has a deviation, the innovation no longer has orthogonality, that is:

[0094]

[0095] Therefore, an innovation non-orthogonality factor is constructed:

[0096]

[0097] Theoretically, when the foot kinematic model completely matches the current true motion state, the motion matching factor mm in,k is 0. As the deviation between the foot kinematic model information and the current true motion state increases, the motion matching factor mm in,k also increases accordingly.

[0098] (2) For the residual factor:

[0099] The residual of the Kalman filter is expressed as:

[0100]

[0101] Ideally, when the observed information has no deviation, E(r k ) = 0, when the observed information has a deviation, E(r k ) = μ, Therefore, a residual factor can be constructed as the motion matching factor:

[0102]

[0103] Theoretically, when the foot kinematic model completely matches the current true motion state, the motion matching factor mm re,k is 0. As the deviation between the foot kinematic model information and the current true motion state increases, the motion matching factor mm re,k also increases accordingly.

[0104] Step S5: Robust fusion algorithm based on online clustering decision.

[0105] The present invention proposes a robust fusion algorithm based on online clustering decision. For alternative fusion moments, it adaptively decides whether to fuse inertial navigation information and kinematic model information according to the motion matching factor. When the matching degree represented by the motion matching factor reaches the set requirement, the kinematic model information and the micro-inertial navigation information are fused to achieve pedestrian autonomous navigation.

[0106] Since the Kalman filter is an iterative process, an online clustering method is designed, that is, in the case where different classes are known to exist, the eigenvalue of each class is updated along with the iterative process, so that the eigenvalues of the same class are highly similar and the eigenvalues of different classes are highly different.

[0107] First, update the online clustering center value according to the following formula:

[0108]

[0109] where is the center value of the normal matching class, is the center value of the abnormal matching class, is the number of the normal matching class, is the number of the abnormal matching class; mm k-1 is the motion matching factor value, and the innovation non-orthogonal factor or the residual factor is selected; the subscripts k and k - 1 represent time, and in the present invention, the innovation non-orthogonal factor mm in,k or the residual factor mm re,k can be selected. The subscripts k and k - 1 represent time.

[0110] Then calculate the matching degree of the kinematic model at this time:

[0111]

[0112] where is the distance between the motion matching factor and the center of the normal matching class; is the distance between the motion matching factor and the center of the abnormal matching class.

[0113] Set the matching degree threshold to 80%.

[0114] If D mm,k ≤80%, it means that the kinematic model at time k does not match the true motion state. Then, at this moment, the inertial navigation information and the kinematic model information are not fused, and the Kalman filter only performs time update, does not perform measurement update, and does not perform inertial navigation error correction:

[0115]

[0116] where, P k-1 is the error variance matrix, and Q k-1 is the process noise variance matrix.

[0117] If D mm,k >80%, it means that the kinematic model at time k matches the true motion state. Then, at this moment, the inertial navigation information and the kinematic model information are fused, and the Kalman filter performs time update and measurement update, and at the same time performs inertial navigation error correction:

[0118]

[0119] The following is a specific implementation process of this step, including the following steps:

[0120] Step 51: Calculate the motion matching factor mm k The distance from the normal matching class center ; Calculate the distance between the motion matching factor and the abnormal matching class center ;

[0121] Step 52: Calculate the matching degree:

[0122] Step 53: Perform a one-step prediction of the filtering state to obtain Perform a one-step prediction of the filtering state error variance to obtain P k,k-1 ;

[0123] Step 54: Judge whether the matching degree D k is greater than the set matching degree threshold. If so, execute steps 55 - 57; otherwise, execute steps 58 and 59;

[0124] Step 55: Let the matching state label at time k indicate that the kinematic model at time k matches the true motion state; the abnormal matching class center value remains unchanged The number of abnormal matching classes remains unchanged Update the normal matching class center value and the number of normal matching classes

[0125]

[0126] Step 56: Calculate the velocity error observation using the kinematic model information:

[0127]

[0128] Step 57: Use the velocity error observation Z k to calculate the filtering state estimate Use to perform feedback correction on the position, velocity, and attitude of the inertial navigation system; complete this round of iteration.

[0129] Step 58: Let the matching state label at time k indicate that the kinematic model at time k does not match the true motion state; the normal matching class center value remains unchanged The number of normal matching classes remains unchanged Update the abnormal matching class center value and the number of abnormal matching classes

[0130]

[0131] Step 59: Do not perform measurement update and do not perform measurement update; complete this round of iteration.

[0132] The above specific embodiments only describe the design principle of the present invention. The shapes and names of the components in this description can be different and are not limited. Therefore, those skilled in the art of the present invention can modify or equivalently replace the technical solutions recorded in the foregoing embodiments; and these modifications and replacements do not depart from the gist and technical solutions of the present invention, and shall all fall within the protection scope of the present invention.

Claims

1. A robust pedestrian autonomous navigation method based on online clustering of motion matching factors, characterized in that: include: Step S1: The micro inertial measurement unit MIMU is fixed to the heel, and the gyroscope sensitive angular velocity modulus is used to determine the candidate fusion time; Step S2: using a rigid body rotation model to uniformly describe the foot landing motion states under different dynamics to construct a foot kinematic model; Step S3: Establishing a foot kinematics / micro-INS integrated navigation model under the Kalman filter framework; Step S4: According to the deviation of the observation information of Kalman filtering, the innovation is no longer orthogonal, and the innovation non-orthogonality factor mm is constructed to measure the orthogonality of the innovation in the foot kinematics / micro-INS integrated navigation model in,k ; According to the observation information without deviation, the residual is zero, and the residual factor mm is constructed to measure the residual size of the foot kinematics / micro-inertial navigation integrated navigation model re,k ; The new information non-orthogonal factor mm in,k and / or residual factor mm re,k As motion matching factor mm k , to characterize the matching degree between the current foot kinematic model information and the real motion state; Step S5: For the candidate fusion time, according to the motion matching factor mm k Perform online clustering decision, in motion matching factor mm k When the matching degree of the representation reaches the set requirements, the foot kinematic model information is used to correct the inertial navigation error during the update process of the Kalman filter, thereby integrating the foot kinematic model information and the micro-inertial navigation information obtained by the MIMU to achieve pedestrian autonomous navigation.

2. The method according to claim 1, characterized in that In step S1, the method of determining the candidate fusion time is: using the rising edge trigger judgment, when the gyroscope sensitive angular velocity modulus value at time k-1 Less than the set quasi-zero speed threshold γ foot , and the gyroscope sensitive angular velocity modulus at time k Greater than the set quasi-zero speed threshold γ foot , determine the k moment as the candidate fusion moment.

3. The method according to claim 2, characterized in that The quasi-zero speed threshold γ foot The value range is 2 to 4 rad / s.

4. The method according to claim 1, characterized in that In step S2, the foot kinematic model constructed is: When the foot is in ground contact: Where n is the navigation coordinate system, is the velocity vector of the foot in the navigation coordinate system at time k; b is the carrier coordinate system, is the lever arm vector from the measuring point to the landing point, is the angular velocity of the measuring point, is the attitude matrix of the measurement point.

5. The method according to claim 4, characterized in that In step S3, the foot kinematics / inertial navigation integrated navigation model is constructed as follows: Based on the foot kinematics model and inertial navigation error equation established in step S2, an ankle-calf kinematics / inertial navigation combined navigation model is established: in That is, the position error δp of the inertial navigation k , speed error δv k and attitude error δφ k As a system variable; Z k =δv k That is, the velocity error is used as the observed value, and the velocity pseudo-measurement provided by the foot kinematic model is used. Calculated, that is is the MIMU solution speed; F k,k-1 is the state transfer matrix, Γ k-1 is the process noise driving matrix, W k-1 is the process noise, H k is the observation matrix, r k is the observation noise; in is the output value of the accelerometer in the navigation coordinate system, T s is the sampling period of MIMU; I 3×3 is a 3×3 identity matrix, O 3×3 is a 3×3 zero matrix.

6. The method according to claim 5, characterized in that In step S4, the innovation non-orthogonal factor is: The residual factor is: Among them, Z k is the observed quantity, H k is the observation matrix, R k The measurement noise variance, is the one-step prediction state, P k,k-1 is the error covariance matrix of the one-step prediction state, is the estimated state at time k.

7. The method according to claim 5, characterized in that Step S5 includes: Update online cluster center values in is the center value of the normal matching class, is the center value of the abnormal matching class, is the number of normal matching classes, is the number of abnormal matching classes; mm k-1 is the motion matching factor value, and the innovation non-orthogonal factor or residual factor is selected; the subscripts k and k-1 represent the time; Calculate the fit of the foot kinematic model: in is the distance between the motion matching factor and the center of the normal matching class, is the distance between the motion matching factor and the center of the abnormal matching class; If the matching degree D mm,k If the set matching degree threshold is not reached, the inertial navigation information and the foot kinematic model information are not integrated at this moment, and the Kalman filter only performs time update without measurement update, and the inertial navigation error correction is not performed at the same time; If the matching degree D mm,k When the set matching threshold is reached, the inertial navigation information and the foot kinematic model information are integrated at this moment: Kalman filtering is used to update time and measurement, and the foot kinematic model information is used to calculate the speed error observation Then estimate the filter state Simultaneous use Perform inertial navigation error correction.

Images