A robust pedestrian autonomous navigation method based on motion matching factor online clustering

Abstract

This disclosure provides a robust pedestrian autonomous navigation method based on online clustering of motion matching factors. Step S1: Determine candidate fusion times using the gyroscope's sensitive angular velocity magnitude; Step S2: Employ a rigid body rotation model to uniformly describe foot landing motion under different dynamic conditions, constructing a foot kinematic model; Step S3: Establish a foot kinematic / micro inertial navigation integrated navigation model within a Kalman filtering framework; Step S4: Construct innovative non-orthogonal factors and residual factors as motion matching factors to characterize the degree of matching between the current foot kinematic model information and the actual motion state; Step S5: For candidate fusion times, propose an online clustering method using motion matching factors to decide whether to fuse inertial navigation information and kinematic model information, thereby achieving robust pedestrian autonomous navigation. Using this invention can improve the ease of use of the system, as well as the navigation and positioning accuracy and robustness under different speeds of motion.

Description

Technical Field

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

[0002] Zero Velocity Update (ZUPT) for foot-attached micro inertial measurement units (MIMUs) is a common algorithm for pedestrian autonomous navigation and localization. The ZUPT algorithm recognizes that there exists a moment when the foot touches the ground where the velocity is zero. Therefore, a zero-velocity detector is used to detect this moment, and the Kalman filter algorithm is run to estimate and correct the inertial navigation system error, thereby suppressing error divergence.

[0003] In traditional architectures, the detection accuracy of the zero-velocity detector is a key factor affecting the final positioning accuracy. Therefore, a great deal of research has been conducted on how to improve the accuracy and adaptability of the zero-velocity detector, and significant results have been achieved. However, these solutions focus on optimizing the zero-velocity detector using prior knowledge of human motion 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 ease of use of the system and improve the navigation and positioning accuracy and robustness under different speeds of motion.

[0005] To solve the above-mentioned 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 miniature inertial measurement unit (MIMU) is fixed to the heel, and the alternative fusion time is determined by using the gyroscope to sense the angular velocity magnitude.

[0008] Step S2: Use a rigid body rotation model to uniformly describe the foot landing motion under different dynamic conditions in order to construct a foot kinematic model;

[0009] Step S3: Establish a foot kinematics / miniature inertial navigation integrated navigation model within the Kalman filtering framework;

[0010] Step S4: Due to the bias in the observation information obtained from the Kalman filter, the innovations no longer possess orthogonality. Therefore, a novel non-orthogonal factor is constructed to measure the orthogonality of the innovations in the foot kinematics / miniature inertial navigation integrated model. Based on the observation information that the residual is zero when there is no bias, a residual factor is constructed to measure the magnitude of the residual in the foot kinematics / miniature inertial navigation integrated model. ; Incorporate non-orthogonal factors into the new information and / or residual factor As a motion matching factor This is used to characterize the degree of matching between the current foot kinematics model information and the actual movement state;

[0011] Step S5: For the candidate fusion time, based on the motion matching factor Perform online clustering decision-making based on motion matching factors. When the matching degree of the representation reaches the set requirements, in the process of updating the Kalman filter, the foot kinematics model information is used to correct the inertial navigation error, thereby fusing 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 candidate fusion time is: using rising edge triggering judgment, when... k angular velocity magnitude of the gyroscope at any moment Less than the set near-zero speed threshold ,at the same time k gyroscope sensitive angular velocity magnitude at time -1 Greater than the set near-zero speed threshold ,Sure k The time is the alternative fusion time.

[0013] Preferably, the quasi-zero velocity threshold The value range is 2~4 rad / s.

[0014] Preferably, in step S2, the constructed foot kinematic model is as follows:

[0015] When the foot is in the process of landing:

[0016]

[0017] in, For the navigation coordinate system, for k The velocity vector of the foot in the navigation coordinate system at any given moment; For the carrier coordinate system, To measure the lever arm vector from the point of measurement to the point of contact, The rotational angular velocity of the measurement point. This is the attitude matrix of the measurement point.

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

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

[0020]

[0021] in That is, the position error of the inertial navigation system Speed ​​error and attitude error As a system variable; That is, using velocity error as the observable, based on the pseudo-velocity measurement provided by the foot kinematic model. Calculated, i.e. ; To improve MIMU calculation speed; Here is the state transition matrix. This is the process noise driving matrix. For process noise, For the observation matrix, To observe noise;

[0022]

[0023]

[0024] in This is the output value of the accelerometer in the navigation coordinate system. The sampling period of the MIMU; It is a 3×3 identity matrix. It is a 3×3 zero matrix.

[0025] Preferably, in step S4, the innovation nonorthogonal factor is:

[0026]

[0027] The residual factor is:

[0028]

[0029] in, For observation purposes, For the observation matrix, Measure noise variance To predict the state in one step, Let the error covariance matrix be the state prediction matrix for one step. for k Estimated state at time.

[0030] Preferably, step S5 includes:

[0031] Update online cluster center value

[0032]

[0033] in It is the center value of the normal matching class. It is the center value of the exception matching class. This is the number of normal matching classes. It is the number of exception matching classes; For the motion matching factor value, select either the innovative non-orthogonal factor or the residual factor; subscript k and k -1 indicates the time;

[0034] Calculate the fit of the foot kinematic model:

[0035]

[0036] in The distance between the motion matching factor and the center of the normal matching class. The distance between the motion matching factor and the center of the abnormal matching class;

[0037] If the matching degree If the set matching threshold is not reached, the inertial navigation information and foot kinematics model information will not be fused at this time. Kalman filtering will only perform time updates and will not perform measurement updates or inertial navigation error correction.

[0038] If the matching degree Once the set matching threshold is reached, the inertial navigation information and foot kinematics model information are fused: Kalman filtering is used for time and measurement updates, and the foot kinematics model information is used to calculate the velocity error observation. And thus estimate the filter state. At the same time, utilize Perform inertial navigation error correction.

[0039] Beneficial effects:

[0040] (1) This invention proposes a robust pedestrian autonomous navigation method based on online clustering of motion matching factors, which simplifies the system design process and effectively improves the navigation and positioning accuracy and robustness under different speeds of motion.

[0041] (2) Using a large threshold quasi-zero velocity coarse detector based on the magnitude of angular velocity effectively reduces the difficulty of parameter adjustment: only the gyroscope is used to sense the angular velocity. As the primary information source for the detector, we avoid introducing acceleration information to prevent negative impacts. To ensure that there is at least one alternative fusion moment in each gait cycle under changing dynamics, we set the detector threshold to a relatively large value. To reduce the number of abnormal fusion moments while ensuring that normal fusion moments occur during the decay phase of angular velocity, we use rising edge triggering correction.

[0042] (3) Constructing novel nonorthogonal factors and residual factors as motion matching factors effectively characterizes the degree of matching between foot kinematics model information and real motion state.

[0043] (4) A robust fusion algorithm based on online clustering decision-making is proposed to improve the adaptability of decision-making to motion matching factors and filtering parameters. Attached Figure Description

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

[0045] Figure 2 This is a schematic diagram of the foot's movement during the landing phase. Detailed Implementation

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

[0047] This invention proposes a robust pedestrian autonomous navigation method based on online clustering of motion matching factors. For example... Figure 1 As shown, it includes the following steps:

[0048] Step S1: Use a simple large-threshold quasi-zero-rate coarse detector to obtain alternative fusion times.

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

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

[0051] Step S4: Construct novel nonorthogonal factors and residual factors as motion matching factors to characterize the degree of matching between the current foot kinematics model information and the actual motion state, providing supporting information for subsequent fusion decisions;

[0052] Step S5: For the alternative fusion time, an online clustering method of motion matching factors is proposed to decide whether to fuse inertial navigation information and kinematic model information, thereby realizing robust pedestrian autonomous navigation.

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

[0054] Step S1: Design of a large-threshold quasi-zero velocity coarse detector based on the gyroscope's sensitive angular velocity magnitude.

[0055] This invention employs a large-threshold quasi-zero velocity coarse detector based on the magnitude of angular velocity. It uses a rising edge triggering correction method, eliminating the need for a sliding window. It is simple, efficient, and easy to use in both design and application.

[0056] Firstly, the foot, as the primary unit of movement, possesses abundant linear and angular motion information. Therefore, a MIMU (Multi-Instrument Unit) incorporating accelerometers and gyroscopes can be used to sense foot motion information. However, when the dynamics of forward movement increase, the foot experiences a significant impact upon landing, causing drastic changes in the acceleration signal. Using acceleration information to construct a detector would have a negative impact. Therefore, this invention only uses gyroscopes to sense angular velocity. As the primary source of information for the detector.

[0057] Secondly, to ensure that there is at least one alternative fusion time within each gait cycle under dynamic changes, the detector threshold is set to a relatively large value, while the window size is set to 1 for ease of calculation. The detector threshold... The preferred value is 2~4 rad / s.

[0058] Meanwhile, to reduce the number of abnormal fusion moments while ensuring that normal fusion moments occur during the angular velocity decay phase, a rising edge triggering correction method is adopted. k Norm of the gyroscope's sensitive angular velocity at any given time Less than the set near-zero speed threshold ,at the same time k The norm of the gyroscope's sensitive angular velocity at time -1 Greater than the set near-zero speed threshold ,Sure k The time is considered a candidate fusion time. That is, a time is considered a candidate fusion time only if the following condition is met:

[0059] (1)

[0060] in for The angular velocity that the gyroscope is sensitive to at any given time. To set the quasi-zero speed threshold.

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

[0062] like Figure 2As shown, the Miniature Inertial Measurement Unit (MIMU) is fixed at point M on the heel, H is the landing point, and T is the toe. This invention uses a rigid body rotation model to uniformly describe the foot's landing motion under different dynamic conditions. Specifically, the motion from foot landing until the foot is fully flat on the ground is modeled as MHT, where the "L"-shaped rigid body rotates around point H. The faster the human body moves, the greater the rotational angular velocity. At this time, the foot kinematic model is:

[0063] When the foot is in the process of landing:

[0064] (2)

[0065] The model expressed at time k is:

[0066]

[0067] in, For the navigation coordinate system, for k The velocity vector of the foot in the navigation coordinate system at any given moment; For the carrier coordinate system, To measure the lever arm vector from the point of measurement to the point of contact, The rotational angular velocity of the measurement point. This is the attitude matrix of the measurement point.

[0068] Step S3: Construct a foot kinematics / micro-inertial navigation integrated model.

[0069] This invention establishes an ankle-lower leg kinematic / inertial navigation integrated model based on the kinematic model and inertial navigation error equation established in step S2, within the Kalman filtering framework:

[0070] (3)

[0071] in That is, the position error, velocity error and attitude error of the inertial navigation system are used as system variables; That is, using velocity error as the observed variable, the velocity is calculated in actual use based on the pseudo-measurement of velocity provided by the kinematic model. . To improve MIMU calculation speed; Here is the state transition matrix. This is the process noise driving matrix. For process noise, For the observation matrix, To observe noise.

[0072] In equation (3):

[0073] (4)

[0074] (5)

[0075] in This is the output value of the accelerometer in the navigation coordinate system. The sampling period is for the miniature inertial navigation system (MIMU). It is a 3×3 identity matrix. It is a 3×3 zero matrix.

[0076] Step S4: Design motion matching factors.

[0077] The candidate fusion times obtained in S1 are only the result of a rough screening using angular velocity. If the foot kinematics model information and micro-inertial navigation information are directly fused at the candidate times, a large error will occur. Therefore, this invention proposes to use a motion matching factor to characterize the degree of matching between the current kinematics model information and the actual motion state, calculate the degree of matching based on the motion matching factor, and make an adaptive decision on whether to fuse.

[0078] The motion matching factor is a scalar or vector quantity used to characterize the degree of matching between the kinematic model and the actual motion state. This invention analyzes the information characteristics of the Kalman process and constructs innovation nonorthogonal factors and residual factors as motion matching factors. It comprehensively calculates the Euclidean distance between observed and predicted information to characterize the degree of matching between the current kinematic model information and the actual motion state. The lower the value of the motion matching factor, the higher the degree of matching.

[0079] (6)

[0080] (7)

[0081] in For non-orthogonal factors of the new information, For residual factor, For observation purposes, For the observation matrix, To measure the noise variance, To predict the state in one step, Let the error covariance matrix be the state prediction matrix for one step. To estimate the state.

[0082] The derivation of constructing the novel nonorthogonal factor and the residual factor is as follows:

[0083] (1) For non-orthogonal factors of innovation:

[0084] In an ideal Kalman filter, the innovation is orthogonal. However, when the observation information is biased, the innovation will no longer be orthogonal.

[0085] New vector It can be represented as:

[0086] (8)

[0087] in, To observe the noise, For one-step prediction error, For one-step prediction observation

[0088] so

[0089] (9)

[0090] in, Let be the variance of the observed values.

[0091] According to the orthogonality theory of new information, we have:

[0092] (10)

[0093] at this time:

[0094] (11)

[0095] When the observation information is biased, the new information no longer possesses orthogonality, that is:

[0096] (12)

[0097] Therefore, a novel nonorthogonal factor is constructed:

[0098] (13)

[0099] Theoretically, when the foot kinematics model perfectly matches the current actual movement state, the movement matching factor... The initial value is 0. As the deviation between the foot kinematics model information and the current actual movement state increases, the movement matching factor decreases. It also increases accordingly.

[0100] (2) For the residual factor:

[0101] The residuals of Kalman filtering are expressed as follows:

[0102] (14)

[0103] Ideally, when the observation information is unbiased, , When the observation information is biased, , Therefore, a residual factor can be constructed as a motion matching factor:

[0104] (15)

[0105] Theoretically, when the foot kinematics model perfectly matches the current actual movement state, the movement matching factor... The initial value is 0. As the deviation between the foot kinematics model information and the current actual movement state increases, the movement matching factor decreases. It also increases accordingly.

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

[0107] This invention proposes a robust fusion algorithm based on online clustering decision-making. For candidate fusion times, it adaptively decides whether to fuse inertial navigation information and kinematic model information based on motion matching factors. When the matching degree represented by the motion matching factors reaches the set requirements, it fuses kinematic model information and micro inertial navigation information to achieve pedestrian autonomous navigation.

[0108] Since Kalman filtering is an iterative process, an online clustering method is designed. This method updates the feature values ​​of each category during the iterative process, given the existence of different categories, so that the features of the same category are highly similar and the features of different categories are highly dissimilar.

[0109] First, update the online cluster center value according to the following formula:

[0110] (16)

[0111] in It is the center value of the normal matching class. It is the center value of the exception matching class. This is the number of normal matching classes. It is the number of exception matching classes; For the motion matching factor value, select either the innovative non-orthogonal factor or the residual factor; subscript k and k -1 represents time, which in this invention can be selected as the innovation nonorthogonality factor. or residual factor Subscript k and k -1 indicates the time.

[0112] Then calculate the fit of the kinematic model at this point:

[0113] (17)

[0114] in , where is the distance between the motion matching factor and the center of the normal matching class; denoted as the distance between the motion matching factor and the center of the abnormal matching class.

[0115] Set the matching threshold to 80%.

[0116] if ,express k If the kinematic model at a given moment does not match the actual motion state, then at that moment, the inertial navigation information and the kinematic model information are not fused. The Kalman filter only updates the time and does not update the measurements, nor does it correct the inertial navigation error.

[0117] (18)

[0118] in, Let Variance be the error matrix. Let be the process noise variance matrix.

[0119] if ,express k If the kinematic model matches the actual motion state at any given moment, then at that moment, the inertial navigation information and the kinematic model information are fused, and Kalman filtering is used for time and measurement updates, while simultaneously correcting inertial navigation errors.

[0120] (19)

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

[0122] Step 51: Calculate the motion matching factor Matching class center with normal distance ; Calculate motion matching factor and abnormal matching class center The distance;

[0123] Step 52: Calculate the matching degree: ;

[0124] Step 53: Perform a one-step prediction of the filtered state to obtain... Perform a one-step prediction of the variance of the filtered state error to obtain... ;

[0125] Step 54: Determine the matching degree If the match is greater than the set matching threshold, proceed to steps 55-57; otherwise, proceed to steps 58 and 59.

[0126] Step 55: Let k Matching status labels at all times ， express k The kinematic model at any given time is matched with the actual motion state; the center values ​​of outlier matches remain unchanged. The number of abnormal matching classes remains unchanged. Update the center value of the normal matching class. Number of normal matching classes :

[0127] ,

[0128] Step 56: Calculate velocity error observations using kinematic model information:

[0129]

[0130] Step 57: Observe using velocity error Calculate the filtered state estimate ,use Feedback corrections are made to the position, velocity, and attitude of the inertial navigation system; this iteration is completed.

[0131] Step 58: Let k Matching status labels at all times ， express k The kinematic model at any given time does not match the actual motion state; the center values ​​of normally matched classes remain unchanged. The number of normal matching classes remains unchanged. Update the center value of the abnormal matching class. Number of exception matching classes :

[0132] ,

[0133] Step 59: Do not perform measurement updates; complete this iteration.

[0134] 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 robust pedestrian autonomous navigation method based on online clustering with motion matching factors, characterized in that, Comprise: Step S1: The miniature inertial measurement unit MIMU is fixed to the heel, and the gyroscopic sensitive angular velocity module value is used to determine the candidate fusion time; The way of determining the alternative fusion time is: using rising edge trigger judgment, when k the moment gyro sensitive angular velocity module value is less than the set quasi-zero speed threshold , at the same time k -1 moment gyro sensitive angular velocity module value is greater than the set quasi-zero speed threshold , it is determined k that the moment is the alternative fusion time; Step S2: A rigid body rotation model is used to uniformly describe the foot landing motion state under different dynamics to construct a foot kinematics model; Step S3: A foot kinematics / micro-inertial navigation integrated navigation model is established under the Kalman filtering framework; Step S4: According to the observation information deviation of Kalman filtering, the innovation no longer has orthogonality, and the innovation non-orthogonal factor is constructed to measure the innovation orthogonality in the combined navigation model of foot kinematics / micro inertial navigation ; According to the observation information, when the deviation is zero, a residual factor is constructed to measure the residual size of the combined navigation model of foot kinematics / micro inertial navigation ; the innovation non-orthogonal factor and / or the residual factor are taken as the motion matching factors to represent the matching degree of the current foot kinematics model information and the real motion state. The innovation non-orthogonal factor is: The residual error factor is: wherein, is an observation of the foot kinematics / micro-inertial integrated navigation model, is an observation matrix, is a measurement noise variance, is a one-step predicted state, is an error covariance matrix of the one-step predicted state, is k is an estimated state at time Step S5: According to the motion matching factor making online clustering decisions, the motion matching factor When the matching degree of the representation reaches the set requirement, the inertial navigation error is corrected by using the foot kinematics model information in the updating process of Kalman filtering, so as to fuse the foot kinematics model information and the micro inertial navigation information obtained by the MIMU, and realize the autonomous navigation of the pedestrian.

2. The method of claim 1, wherein, the quasi-zero velocity threshold The value range is 2-4 rad / s.

3. The method of claim 1, wherein, In step S2, the constructed foot kinematics model is: When the foot is in landing motion: wherein, is a navigation coordinate system, is k is a velocity vector of the foot in the navigation coordinate system at the time instant; is a carrier coordinate system, is a lever arm vector from the measurement point to the destination point, is a rotational angular velocity of the measurement point, is an attitude matrix of the measurement point.

4. The method of claim 3, wherein, In step S3, the constructed foot kinematics / inertial navigation integrated navigation model is: Based on the foot kinematics model and the inertial navigation error equation established in step S2, a ankle-calf kinematics / inertial navigation integrated navigation model is established: wherein i.e. position error of inertial navigation , velocity error and attitude error as system variables; i.e. velocity error as observation, velocity pseudo-measurement provided by foot kinematics model is calculated, i.e. ; velocity is solved by MIMU; state transition matrix is process noise driving matrix is process noise is observation matrix is observation noise is wherein is an output value of the accelerometer in the navigation coordinate system, is a sampling period of the MIMU; is a 3x3 identity matrix, is a 3x3 zero matrix.

5. The method of claim 4, wherein, Step S5 includes: Updating the online clustering center value wherein 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; is the motion matching factor value, selecting the innovation non-orthogonal factor or the residual factor; subscript k and k -1 represents the time point; Calculating the matching degree of the foot kinematics model: wherein is the distance of the motion matching factor to the center of the normal matching class, is the distance of the motion matching factor to the center of the abnormal matching class; If the matching degree If the matching degree does not reach the set matching degree threshold, the inertial navigation information and the foot kinematics model information are not fused at this moment, the Kalman filtering only performs time updating, does not perform measurement updating, and does not perform inertial navigation error correction. If the matching degree Once the set matching threshold is reached, the inertial navigation information and foot kinematics model information are fused: Kalman filtering is used for time and measurement updates, and the foot kinematics model information is used to calculate the velocity error observation. And thus estimate the filter state. At the same time, utilize Perform inertial navigation error correction.

Images